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ABSTRACT 


Accurate  track  keeping  oTauntonomous  iinderwater  vehicles  is. necessary  for  =the 
autonomous  navigation  of  a  vehicle  through  confined;  spaces,  and  in  the  presence  of 
obstacles  and  cross-current  environments.  Uncertainties  in  the  force  coefficients  and 
environmental  disturbances,  as  well  as  the  required  accuracy  lead  to  the  need  for  a  robust 
sliding  mode  control  for  successful  vehicle  operations.  This  thesis  investigates  the  use  of 
a  cross  track  error  guidance  law  with  a-sliding  mode  compensator  and  presents  results 
based  on  cornputer  simulations  using  a  nonlinear  dynamic  model  of  the  Swimmer  Delivery 
Vehicle.  Steady  state  errors  and  stability  requirements  are  evaluated  analytically  for  a 
given  current  speed  and  direction,  and  are  confinhed  through  numerical  integrations.  The 
use  of  integral  control  and  disturbance  estimation  and  compensation  methods  are 
developed -in  order  to  achieve  the  desired,  steady  state  accuracy.  A  leading  track  control 
monitoring  technique  is  used  to  eliminate  track  overshoot  during  turning  and  reduce  the 
rudder  activity.  Finally,  the  effects  of  measurement  noise  are  evaluated  and  guidelines  are 
developed  for  suppressing  them. 
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1.  INTRODUCTION 


A.  BACKGROUND 

Autpnomous:underwater  vehicles  (AUV’S)  arc  generating  much  interest  in  the 
U.  S.  Navy  and  major  private  defense  corporations.  As  monetary  and  budgetarj' 
constraints  dominate  the  force  structure  of  the  armed  forces,  intelligent  unmanned 
underwater  vehicles  become^a  highly  attractive  alternative  to  manned  submarines  [Ref. 
Ij.  The  AUV  Can  be  downloaded  with  a  myriad  of  unclassified  missions,  i.e., 
reconnaissance,  AS W,  decoy,  survey,  ocean  engineering;  for  a  fraction  of  the  cost  of  a 
manned  submarine  for  the  same  missions.  In  order  for  the  AUV  to  cany'  out  these 
missions,  the  AUV  should  be  able  to  operate  freely  in  the  ocean  environment  with  respect 
to  speed,  heading  and  depth.  Such  operational  requirements  have  to  be  easily  and  reliably 
accomplished  in  the  presence  of  environmental  and  physical  uncenainties.  Autopilot 
design  becomes  then  an  integral  and  important  aspect  of  overall  AUV  design  [Refs.  2,  3, 
4  and  5]. 

The  autopilot,  which  controls  the  AUV  with  regards  to  a  commanded  direction 
and/or  depth,  is  subjugated  to  a  global  planner,  which  monitors  and  directs  the  AUV  in 
a  global  sense.  All  information  concerning  the  environment  of  the  AUV  is  detected  by 
the  sensing  instrumentation  onboard  the  AUV  and  sent  to  the  higher  level  intelligence 
systems,  such  as  the  global  planner  and  the  autopilot,  so  that  its  missions  may  be  carried 
out.  The  dynamics  of  underwater  vehicles  are  described  by  highly  complc,x,  nonlinear 
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systems  of  lequations  with  uncertain  coefficients  and  disturbances  that  are  difficult  to 
measure  [Refs.  6, 7  and  8].  A  completejsix  degree  of  fireedpm  modellfor  the  underwater 
nhaneuvering  of  the  AUV  is  utilized  in  which  the  hydrodynamic  force  coefficients  are 
taken  from^previous  studies  of  a  swimmer  delivery  vehicle;[Refs.  9  and  lOj.  Recently, 
the  development  of  variable  structure  control  in  the  form  of  sliding  mode  control  has  been 
shown  to  provide  added  robusmess  that  is  quite  remarkable  for  AUV  autopilot  design 
[Ref  11].  Robust  control  using  sliding  mode  control  provides  accurate  control  of 
nonlinear  systems  despite  unmodelled  system  dynamics,  thus  making  it  a  highly  likely 
prospect  for  designing  the  control  laws  and  guidance  methods  that  will  govern  the 
autopilot  function  of  unmanned  vehicles. 

B.  OBJECTIVE  OF  THIS  THESIS 

After  developing  the  necessary  sliding  mode  control  theory,  the  objective  of  this 
thesis  is  to  investigate  the  use  of  a  cross  track  enror  guidance  law  with  a  sliding  mode 
compensator  and  to  present  results  based  on  computer  simulations  using  a  nonlinear 
dynamic  model  of  a  swimmer  delivery  vehicle.  Various  control  methods  will  be 
iiivestigated  for  use  in  the  sliding  mode  based  cross  track  error  guidance  law.  In  the 
development  of  one  of  the  control  methods,  a  current  observer  will  be  developed  and 
shown  to  work  well.  This  current  observer  is  direly  needed  because  the  current  will  be 
used  as  a  constant  disturbance  in  this  control  law  and  must  somehow  be  determined. 
Since  the  lateral  current  for  each  track  is  used  and  cannot  be  measured  for  every  track  for 
all  times,  then  the  current  must  be  estimated  or  observed  from  parametcre  that  can  be  measured. 
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After  different  confrol  methods  have  been  developed  and  their  results  presented,  a 
leading  track  control  monitoring  technique  will  be  developed:  This  technique  can  be  used 
with  each  of  the  control  methods  presented.  It  will  be  shown  that  this  technique  will 
automatically  initiate  the  turn  onto  the  leading  track,  taking  into  account  the 
environmental  conditions,  with  no  overshoot  and  optimal  use  of  the  nidder. 

Finally,  noise  will  be  introduced  into  the  measurable  parameters  and  the  effects  will 
be  evaluated.  Guidelines  will  then  be  developed  for  suppressing  the  effects  of 
measurement  noise.  From  all  this,  conclusions  will  be  made  and  recommendations  will 
be  developed  for  a  highly.robust  and  effective  system  for  controiling;the  next  generation 
of  autonomous  underwater  vehicles  under  construction  at  the  Naval  Postgraduate  School 
and  elsewhere  in’private  industry'. 

C.  THESIS  OUTLINE 

In  Chapter  2,  the  sliding  plane  and  gain  coefficients  to  be  used  as  the  basis  for 
developing  an  AUV  autopilot,  using  tlie  sliding  mode  control  theory,  arc  developed.  The 
equations  of  motion  to  be  used  only  in  the  horizontal  plane,  or  the  path  keeping  aspect 
of  the  AUV,  are  presented. 

Chapter  3  develops  a  straight  line  track  that  becomes  the  reference  from  which  the 
cross  track  error  is  measured.  The  track  nomenclature  and  geometry  to  regulate  the  error 
in  deviation,  or  cross  track  distance,  from  the  nominal  straight  line  track  is  presented. 

Chapter  4  develops  the  integral  control  method.  The  effects  of  adding  integral 
control  to  eliminate  the  steady  state  error  for  a  single  way  point  and  for  multiple  way 
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pointsj^e  investigated.  A  modified  integral  control  method  is  developed  and  results  arc 
presented. 

Chapter  5  develops  a  disturbance  compensation  method,  for  perfect  current  input, 
and  a  disturbance  estimation  and  compensation  method,  for  estimated  orobser\'cd  current 
input.  Since  the  lateral  current  to  each  track  is  used  as  a  constant  disturbance  in  the 
control  law,  then  the  lateral  current  must  be  able  to  be  determined  for  each  track.  The 
only  way  to  determine  the  lateral  current  for  each  track  is  to  develop  a: current  obser\'er 
using measurable  parameters  from  onboard  sensors.  This  current  observer  is  developed 
and  results  arcrpresent^  in  this  chapter. 

Chapter  6  investigates  a  technique  referred  to  as  leading  track  control  monitoring, 
which  utilizes  the  leading  track  to  automatically  initiate  the  turn  onto  the  leading  track, 
within- the  physical  constraints  of  the  AUV  and  taking  into  account  the  environmental 
conditions.  This  technique  initiates  the  turn  so  as  not  to  overshoot  the  leading  track  and 
to  optimize  the  use  of  the  rudder,  within  the  vehicle  contsu-aints.  Results  of  this 
technique  are  presented. 

Chapter  7  introduces  noise  into  the  measurable  parameters  and  the  effects  arc 
evaluated.  Guidelines  for  suppressing  the  effects  of  measurement  noise  are  put  fonh  in 
this  chapter. 


II.  EQUATIONS  OF  MOTION  AND  SLIDING  PLANE  DEVELOPMENT 

A.  INTRODUCTION 

Prior  to  beginning  a  discussion  on  cross  crack  error  guidance  control  law,  an  in 
depth  overview  of  the  sliding  plane  and  gain  coefficients  for  use  in  regulating  the  steady 
state  error  in  deviation  from  the  nominal  straight  line  track  needs  to  be  developed.  Also, 
a  development  of  the  equations  of  motion  used  in  this  research  will  be  conducted.  The 
main  assumption  to  be  made,  at  the  beginning,  is  that  only  the  horizontal  plane,  or  the 
steering  aspect  of  the  AUV,  will  be  considered  throughout  this  research.  This  assumption 
is  based  upon  the  previous  work  done  on  the  Line  of  Sight  (LOS)  guidance  control  law 
by  Lienard  [Ref.  12],  where  it  was  esrablishcd  that  heading,  speed  and  depth  sliding  mode 
autopilots  could  be  designed  independently.  The  remaining  pan  of  this  chapter  will 
develop  the  equauons  of  morion  for  the  AUV  used  in  thjs  research  and  will  also  develop 
the  sliding  plane  tmd  gain  coefficients  to  be  used  as  the  basis  for  developing  an  .AUV 
autopilot  using  the  siiding.mode  control  theory. 

B.  EQUATIONS  OF  MOTION 

Intead  of  an  e.xact  set  of  equations  of  morion  for  a  rigid  body  moving  in  a  fluid,  a 
simplified  linear  set  of  equations  of  morion  was  chosen  to  be  used  for  control  design. 
Tne  full  sets  of  nonlinear  equations  of  motion  for  the  AUV  were  taken  from  the  work 
done  previously  at  NPS  by  Boncal  (Ref.  2],  who  used  the  dynarnic  mods!  as  established 


by  Crane,  Summey,  efeal  [Ref.  10],  as  represen tativc' of  the  SDV  Mark  9  vehicle.  The 
SDV  Mark  9  vehicle  is  different  than  the  AUV  currently  being  used  at  NPS,  but  it 
remains  a  useful  vehicle  for  the  study  of  dynamics  and  control  issues.  Since  the  current 
NPS  AUV  does  not  yet  have  validated  hydrodynamic  coefficients,  the  SDV  Mark  9 
vehicle  serves  our  purpose  in  developing  guidance  control  laws  that  can  apply  to  the  NPS 
AUV  or  any  vehicle  of  choice. 

It  is  too  time  consuming  for  an  onboard  computer  to  try  and  control  ah  underwater 
vehicle  using  an  exact  set  of  equations  of  motion,  therefore,  a  linearized  set^of  equations 
oTmotion  was  developed.  By  restricting  the  motion  ofthe  vehicle  to  the  horizontal  plane, 
only  the  equations  of  motion  in  the  horizontal  plane  will  be  developed.  In  fact,  this 
research  utilizes  the  assumptions  and  equations  of  motion  done  previously  by  Lienard 
[Ref  12].  The  state  space  form  that  can  bemsed  for  heading  control  is 

\j|f  =  r  (2.1a) 

V  =  a^iUV  +  +  jb.u^5 

r  =  a^^uv  +  agjur  + 

with  =  -0.04538,  =  -0-35119,  =  -0.002795,  =  -0.09568, 

=  0.011432  and  =  -0.04273,  upon  which  the  control  laws  can  be  based  utilizing 


(2.1b) 

(2.1c) 
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sliding  mode  control  theory-  To  regulate  the  error  in  deviatipn  from  a  nominal  stfaight 
line  track,  the  following  equation  is  introduced] 


and-when  linearized 


y  =  vcos\\f  +  usin)^ 


(2.2) 


y  =  V  +  M\}f  {no  current),  (2-3) 

where  y  denotes  the  cross  track  distance  off  the  nominal  track.  So,  the  state  space  fonn 
to  be  used  for  cross  track  error  control,  with  no  current,  is 


\j/  =  r  (2.4a) 

V  =  a^^uv  +  a^^ur  +  b^u'^h  (2.4b) 

r  =  fljjMV  +  a^^ur  +  (2.4c) 

y  =  V  +  .M\j/  (2.4d) 

at  any  nominal  u. 

The  system  equations  of  (2.1)  and  (2.4)  will  be  used  for  the  controller  design, 
whereas,  the  equations  developed  by  Lienard  [Ref.  12]  for  the  nonline^  steering  equations 
wilhbe  used  to  simulate  the  AUV  in  all  trial  runs. 
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C.  SLIDING  PLANE  AND  GAIN  COEFFICIENT  DEVELOPMENT 

1.  Pole  Placement  Method 

Since  the  vehicle  motion  is  based  on  linearized  differential  equations  of 
motion,  then  the  nature  of  the  motion  of  the  vehicle  can  at  best  be  only  approximated. 
Now,  any  control  law  based  on  an  approximate  plant  model  must  be  robust  enough  to 
ensure  stability  and  acceptable  transient  response  characteristics  in  the  presence  of 
parameter  variations  and/or  unmodeled  dynamics  [Ref.  13].  Since  the  parameters  and 
coefficients  are  valid  for  the  nonlinear  model  of  the  SDV  Mark  9  vehicle  and  a  new  set 
of  parameters  and  coefficients  has  still  to  be  verified  for  the  NPS  AUV,  then  there  will 
definitely  exist  parameter  variations,  unmodelled  dynamics,  and  disturbances.  Sliding 
mode  control  laws  provide  effective  and  robust  ways  of  controlling  uncertain  plants. 
Sliding  mode  control  utilizes  a  high  speed  switching; controldaw  to  drive  the  plant’s  state 
trajectory  on  to  a  sliding  plane  for  all  subsequent  times.  The  control  law  will  be  based 
upon  the  linear  model 

X  =  [A]x  +  [b]u  (2-5) 

where 

=  [\|/,v,r,y],  =  [0,0:4316,-0.1538,0],  m  =  5 

and 
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lA] 


0  0  10 

0  -0.2723  -2.1071  0 
0  -0.0168  -0.1538  0 
6  1  0  0 


For  the  four  dimensional  system  (2.4),  the  sliding  plane  is4he  Euclidean  space 


a(x)  =  0  where  =  0,  (2.6) 

and4he  coefficient  is  arbitrai7.  Equation  (2.6)  can  be  written  as 


s'^x  =  0  with  5^  =  ^2,  ^3,  .  (2.7) 

Determining  s  will  determine  the  sliding  plane- uniquely.  The  control  law  has  to  be  able 
to  drive  system  (2.1)  onto  the  sliding  plane  (2.6)  for  an  arbitrary  choice  of  initial 
conditions.  By  defining  the  Lyapunov  function 

V(a:);  =  i(o(A:)]^  (2.8) 

asymptotic  stability  of  (2‘8)  is  guaranteed  provided  V(x)  is  a  negative  definite  function, 
or 

V  =  ad  =  -ti^afx), 

such  that, 


d  =  -Ti^^/g/ifd). 

Since  o(x)  =  s^x,  system  (2.1)  and  equation  (2.5)  can  be  used  to  get 
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s\Ax  +  bu)  =  -TiV/gn(a), 


and  solving  for  u: 


u  -  -(s^b)7^s^Ax  -  r[\s  ^b)~^signi(j), 


or, 


u  =  u  +  u.  (2.10) 

=It  is  important  to*recognize  that  the  feedback  control  law  u  is  composed  of  two  parts. 
The  first, 


u  =  -{s^by^s^Ax 

is  a  linear  feedback  law,  whereas  the  second. 


(2.11) 


u  =  -lyis^by^signic)  (2.12) 

is  a  nonlinear  feedback  with  its  sign  toggling  between  pus  and  minus  according  to  which 

side  of  the  sliding  pane  the  system  is  located  on.  Since  u  has  to  change  its  sign  as  the 

system  crosses  c{x)  =  0,  the  sliding  surface  has  to  be  a  hyperplane  (dimension  of  one 

less  than  the  state  space).  It  is  u  which  is  mainly  responsible  for  driving  and  keeping 

the  system  onto  the  sliding iplane,  Qix)  =  0  (where  m  =  0  as  well).  Provided  the  gain 

has  been  chosen  large  enough,  u  can  provide  the  required  robustness  due  to 
momentary  disturbances  and  unmodelled  dynamics  without  any  compromise  in  stability. 
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The  linear  feedback  law  (2;1 1)  is  designedisuch  th?.t  the  system  has  the  desired 
dynamics  omthe  sliding  plane.  Since  a{x)  =  0,  then  in  this  case 

u  =  u  -  ^Ax, 

and  the  closed  loop  dynamics  of  (2.2)  are  given  by 

X  =  [A  -  b(s'^Byh'^A]x  (2-13) 

or, 

Jc  =  (/i  -  bk)x,  (2-14) 

where  the  gain  vectorik  can  be  found  from  standard  pole  placement  methods.  The  closed 
loop  dynamics  matrix 

/I  =  A  -  bk  (2.15) 

C 

has  eigenvalues  specified  for  desirable  response.  One  of  the  eigenvalues  of  A^  must  be 

specified  to  be  zero.  This  is  consistent  with  the  decomposition  in  (2.10).  The  linear 
feedback  «  provides nhe  desired  dynamics  on  the  sliding  plane  only.  Therefore,  w  has 
no  effect  in  a  direction  perpendicular  to  cr(x)  =  0.  With  .specified  and  k  computed 
by  pole  placement,  s  can  be  determined  from  (2.13)  and  (2.14), 

k  =  (s^by^s'^A, 


and 


s\A  -  bk)  =  0, 


(2.16a) 


or. 
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(2.16b) 


=  0. 

Therefore,  s  is  a  left  eigenvector  of  that  conesponds  to  the  zero  eigenvalue.  With 

this  choice  of  s,  the  sliding  :plane,  =  0,  and  the  feedback  control  law  (2.10)  are 

completely  deterriuned.  It  should  be  pointed  out  that,. in  applications,  the: states  x, , 
and  x^  are  to  be  interpreted  as  errors-between  the  actual  values  of  t]/,  v,  r,  y  and- 
their  set  points. 

There  are  two  problems  that  arise  from  using  this  approach  of  pole^placemenr 
in  finding  s.  First,  there  is  no  guarantee  that  the  eigenvector  for  s  will  always  be  real. 
Second,  for  multiple  input  systems,  this  approach  will  not  work,  since  more  than  one  pole 
can  not  be  placed  at  the  origin  in  order  to  find  Sireliably.  For  this  research,  these  two 
problems  did  not  play  a  major  factor,  however,  other-methods  were  investigated. 

2.  Coordinate  Transformation  with  Pole  Placement  Method 

An  alternate  approach  that  accounts  for  the  two  problems  stated  in  the  pole 
placement  method  is  to  perform  a  coordinate  transformation  and  to  find  the  corresponding 
transformation  matrix  [Refs.  14  and  15]. 

Define  a  coordinate  transformation, 

y  =Tx  , 

where  T  is  an  orthogonal  n  by  n  transformation  matrix  such  that 
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Tb  - 


0 


(2.1^ 


whereZ7j  is  m;by  m  and  0  is  Cn  -  m)  by  m.  The  nv'^iber  of  states  is  n  and  the  number  of 
inputs  is  m.  In  this  research  m  =  1.  To  del't;**:!'.  ; .  use  the  QR  factorization  of  b, 
where  b  is  decomposed  into  die  form 


b  =  Q 


(2.18) 


and  Q  is  orthogonal  and  R  is  the  upper  triangular.  Now,  fromi(2.17)  and  (2.18), 


T  =  (2  ''  . 

Fromnhe  coordinate  transformation, 


y  =  Tx  , 


then, 


a:  = 


> 


and, 


y  =  Tx  , 

and  when  substituted  into  (2.2),  a  linear  model  is  obtained  in  the  transformed  variable  y, 


y  =  TAT^y  +  Tbu  , 


(2.19) 


The  sliding  condition, 


sh  =  0  , 


becomes 


s^T^y  =  0  , 


or 
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c^y  =  0  , 

with  C  =  Ts.  Performing  a  partition.on  y  and  G, 


(2.20) 


y  = 


where  yj  is  o>ie  by  one  and  yj  is  (ir  -  i)  by  one,  and 


where  C,  is  one  by  one  and  is  (n  -  1)  by  one,  so  that  the  state  equations  in  the 
transfonned  variable  become 


=  ^11-  (2.2 1  a) 

h  =  ^2,}’,  +  ^22>’2  •  (2.21b) 


The  sliding  nlane  (2.20)  now  becomes 


14 


>"1  +  W^>’2  =  0  , 


(2.22) 


with/C,  normalized  to  one.  For  the  sliding  plane  to  be  completely  and  uniquely  defined, 


then  C,  needs  to  be  determined. 


Again  by  defining  the  Lyapunov  function 


V(y)  =  .i(a(>-))^  , 


(2.23) 


asymptotic  stability  of  (2.23)  is  guaranteed  provided  V(y)  is  a  negative  definite  function, 


^(y)  =  ad  =  -Ti^a(>’)  , 


d  =  -ri^signio)  . 


(2.24) 


Differentiating  the  equation  for  the  sliding  plane  (2.20):and  equating  to  (2.24), 


y,  +  =  -11^5/5/1(0)  . 

Substituting  (2.21a)  and  (2.21b)  into  (2.25)  and  solving  for 


(2.25) 


u  =  u  +  u  , 


^2^('^2I>’i  +  '^22>’2)  =  (O) 


«  =  +  ciA2i)y\  +  (/1 ,2  + 


(2.26a) 
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u  =  signio)  ,  (2.26b) 

From  the  sliding  plane  design,  it  is  desired  to  have  a(y)  =  0=  .  which  gives  m  =  !0  andtr  =  u. 

Solving  (2.22)  for  y^, 


=  -C/y,  .  (2.27) 

Equations  on  the  sliding  plane  becorne 

y,  =  ^„y,  +  +  b,u  . 

and  substitutingrfor  u  from  (2.26a), 

y,  =  -C^(A^^y^  +  A^^)  , 

and  substituting  once  more  for  yj  by  differentiating  (2.27), 

-C^y^  =  ■» 

a  linear  combination  of  the  second  set 

h  ?  ^  '422>'2  • 

The  (n  -  1)  independent  equations  on  the  sliding  plane  are 

^2  "  ^21^1  ^23^2  > 

and  substituting  for  y^  from  (2.27) 

y^  =  (A22  -  A^fz)yi  • 

Again  using  pole  placement  of  (n  -  1)  poles,  can  be  determined  and  thus  the  sliding 


plane  is  uniquely  and  completely  determined. 


16 


The  system  matrix  in  the  y  equation  has  rank=(n  - 1)  and  is  a  singular  matrix, 
therefore,  one  poleas  already  at  the  origin.  Only  (n  -  l)^poles  need  to  be  deteirnjned  by 
pole  placement  andithe  two;probIems  from  the  previous  pole  placement  method  have  been 
resolved. 


3.  Linear  Quadratic  Regulator  Coordinate  Transforrnation 

Instead  of  pole  placement  to  detennine  the  sliding  plane  and  gain  coefficients, 
the  Linear  Quadratic  Regulator  (LQR)  with  a  coordinate  transformation  was  investigated. 
This  LQR  method  involves  minimizing  a;  cost  functional  in  which  the  integrand  is  a 

quadratic  function  of  the  state  x(-)  [Refs  14  and  15],  such  as 


/  =  U  (Ar^'Or)  *  . 

Using  the  coordinate  transformation,  y  =  tx, 

/  =  , 

and  by  partitioning 


TQT^  = 


~Q\\  Q-n 

Qix  Qn_ 


(2.29) 


the  cost  functional  becomes 

•^  =  -ij  (yi^i2„y,  +  >2^(2,, y,  +  dt  . 


Nowdefining 


Q  -  G22  ~  Qii 


(2.30a) 


A*  =  -  4,!2„-‘|2,2 

^  =  y,  +  QnQx-^1  . 

a  new  cost  functional,  I,  is  obtained 

I  =  4/  +  v’-e„v)  * 

and 

>2  =  '^*>2  *  • 


(2.30b) 


(2.30c) 


(2.31) 


The  problem  is  now  to  minimize  I  subject  to  (2,31).  However,  in  order  to 
minimize  I,  an  arbitrary  choice  of  Q*  needs  to  be  made.  The  choice  of  Q‘  greatly 

influences  whether  right  control  or  soft  control  will  be  obtained,  but  it  provides  no  easier 
a  method  to  obtain  the  sliding  plane  and  gain  coefficients.  For  the  remainder  of  this 
research,  the  pole  placement  method  is  chosen  to  determine  the  sliding  plane  and  gain 
coefficients  with  closed  loop  poles  as  specified  for  the  parricular  trial  run. 
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m.  TRACK  DEVELOPMENT 

A.  INTRODUCTION 

Now  that  the  foundation  in  sliding  mode  control  theor>'  has  been  laid  and  the 
method  for  determining  the  sliding  plane  and  gain  coefficients  is  known,  a  straight  line 
track  needs  to  be  developed.  It  will  be  the  perpendicuto  distance  off  this  straight  line 
track  that  will  be  defined  as  the  cross  track  distance,  y.  This  cross  track  distance  will  be 
the  object  of  the  sliding  mode  control  laws  so  that  this  cross  track  distance  will  be 
controlled  to  zero.  When  the  cross  track  distance  is  zero,  then  the  vehicle  is  on  the 
directed  track  specified  by  the  global  planner.  This  chapter  will  develop  the  track 
nomenclature  and  geometry  in  order  to  regulate  the  error  in  deviation,  or  cross  track 
distance,  from  the  nominal  straight  line  track.  Also,  this  chapter  will  show  that,  at  steady 
state  conditions,  a  steady  state  error  exists  in  the  presence  of  a  current  and  how  the  value 
of  affects  the  stability  of  the  controller.  It  will  be  the  elimination  of  this  steady  state 

error  that  the  various  guidance  controls  laws  to  be  developed  will  concern  themselves. 

B.  NOMINAL  STRAIGHT  LINE  TRACK 

In  order  to  construct  the  nominal  straight  line  track  to  be  used  to  measure  the  cross 
track  distance,  the  global  planner  needs  to  input  two  way  points,  a  staning  point  and  a 
destination  point.  For  this  research,  the  two  way  points  must  be  in  global  coordinates 


19 


(X,Y)  and  in  terms  of  ship  lengths.  The  following  equations  deteimine  the  inertial 
position  rates  of  the  AVW 

X  =  C4  +  ucos\\f  -  vsiny^ 

Y  =  +  usin\^  +  vco.y\|/ 

where  and  are  the  absolute  current  velocities  in  the  global  reference  frame.  The 

angle  a,  measured  from  the  horizontal,  will  define  the  track  for  the  two  way  points  of 
interest.  The  perpendicular  distance  y,  in  local  coordinates,  will  be  tlie  cross  track 
distance  that  willsbe  controlled; such  that  when  the  cross  track  distance  is  zero,.then  the 
vehicle  is  on  the  desired  track.  The  cross  track  distance,  for  this  research,  will  be 
designated,  y,  and  the  distance  along  the  track  will  ^be  designated,  x.  Both  y  and  x  are 
in  local  coordinates.  The  current  position;  of  the  vehicle  will  be  designated  by,  X  and  Y, 
both  in  global  coordinates. 

1.  Geometry  of  a  Nominal  Straight  Line 

Figure  1  will  be  used  to  develop  a  nominal  straight  line  track,  and  it  will  be 
repeated  as  the  vehicle  goes  from  way  point  to  way  point.  The  equations  to  tfansfonn 
the  global  coordinates  into  the  local  coordinates  are 

X  =  Xcosa  +  Ysina 
y  =  Ycosa  -  Xsina  . 

Also,  the  equations  to  translate  global  currents  into-local  currents:  are 
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u  =  cosa  +  V  sina 

c  e-  c  - 

V  =  V  cosa  -  U  sina  . 

C  C  c 

2.  Nomer.dature  of  a  Nominal  Straight  Line  Track 

The  following  nomenclature  will  be  used  in  developing  a  nominal  straight  line 
track  for  thisjfesearch: 

•  y:  the  perpendicular»distance  off  the  track,  indocal  coordinates. 

•  x:  the  distance  along  the;  track,  in  local  coordinates. 

•  XTIME:  desired  total  time  to  go  from  the  stming  point  to  the  destination 

point,  in  seconds. 

•  UREQ:  the  speed  required  to  getcfrom  the  starting  point  to  the  destination  point  in 

the  desired  time,  in  ft/sec. 

•  Xj.:  the  total  length  of  desired  track,  in  feet. 

•  (X,y):  the  current  vehicle  position,  in  global> coordinates. 

•  (X^,y^):  the  destination  way  point,  in  global  coordinates  and  in  ship  lengths. 

•  (X^,yQ):  the  starting  way  point,  in  global  coordinates  and  in  ship  lengths. 

•  a:  the  angle  measured  from  the  horizontal  to  the  line  between  the  starting 

point  and  the  destination  point. 
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From  the  geometry,  the  following  parameters  will  be  defined: 


Xj  = 


Y  -  Y 
■'n  ■'o 

sina 


•  a  =  tan 


rl 


Y  -  Y 


y  =  (Y  -  y^)cosa  -  (X  -  3^Q)sina 


•  X  =  {X  -  XQ)co%a  +  (y  -  yo)sina  . 


UREQ  = 


XTIME 


C.  STEADY  STATE  ERROR 

In  the  presence  of  a  current,  it  has  been  observed  that  a  steady  state  error  will 
occur,  with  no  control,  for  the  linearized  set  of  equations  of  motion  for  the  AUV  at  steady 
state  conditions.  The  linearized  set  of  equations  of  motion  for  the  AUV,  with  no  integral 
control,  was  developed  in  equation  (2.1).  To  account  for  the  current  that  is  perpendicular 

to  the  track,  v^,  equation  (2.3)  is  modified  to 

y  =  V  +  M\|/  +  .  (3.1) 

At  steady  state. 
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\\f  =  V  =  r  =  y  =  0 


(3.2) 


SQ4hat 


0  =  r,  (3.3a) 

0  =  a^uv^  +  ;(3.3b) 

0  =  a2^uv^  +  03c) 


0  =  (3.3d) 

where  a„,  dj,,  £>,,  d^-and  u  are: as  defined  in  Chapter  Two.  The  subscript  s  represents 

the  value  of  the  variable  at  steady  state. 

Since  a,,,  a2v  ^2  nonzero,  then  =  5^  =  0.  Therefore, 


^  sin 


-I 


r  ^ 

V 


(3.4) 


and  when  this  is  substituted  into  the  sliding  plane  and  rudder  equations,  a  steady  state 
error  will  develop.  For  rudder  control, 


5  =  /:,\|/  +  k^  v  +  r  +  k^satsgn{  a)  (3.5) 

and  for  the  sliding  plane. 
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Pi  . 

=  ---sin  ’ 


f 

( 

V, ; 

;+  5, sin"’ 

-c 

u 

U  i 

J 

j 

and 


1 


P, 


+ 


sin 


-1 


f  \ 

V 


v“y 


(3.12) 


Equation  (3.12)  represents  the  steady  state  error  in  the  crossitrack  distance  that  results  in 
the  presence  of  a  lateral  current.  This  steady  state  track  error  can  be  rnade  smaller  by 
increasing  the  value  of  the  nonlinear  gain  but  it  can  never  become  zero.  For  very 

large  is  still  bounded  by 


*^1  .  -i 

y  =  —  sm  * 


The  above  analysis  is  valid  if 


a 


satsgn{<j)  -  —  , 
4> 


which  requires  that  p^cwg«(a)  |  ^  1.  This  requirement  yields  the  necess^  critical  value 
of  k  for  stability,  from  (3.10) 


k  >  k  .= 

n  cnt 


(  \ 


Zt.sin 


-1 


(3.14) 


If  the  nonlinear  gainiis  not  selected  large  enough;  i.e.,  k^  <  then  the  controller 
cannot  guarantee  stability. 
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The  above  analytical  results  can  be  confirmed  by  numeiicar  simulations  of  the  full 
nonline^,  six  degree  of  freedom: model  of  the  SDV.  Closed  loop  poles  on  the  sliding 
plane  were  selected  at  [-0.35,-0.36,-0.37],.  with  (j)  =  0.5  and  u  =  6ift/sec.  This  gives 

fi  =  0.9556ijf  -  0.1085V  +  1.2286r  +  \satsgnio) 


o  =  2.9805 ij;  +  0.2199v  +  3.4445r  +  0.0700y  .  (3.16) 

Figure  2  shows  how  the  controller  works;  with  no  current.  The  way  point  is  selected  at 
(,X,Y)  =  (20,20)  ship  lengths.  In  Figure  2  and  all  subsequent  similar  figures,  the 
following  variables  are  displayed:  X  vs  Y  position,  rudder.angle  vs  time,  the  cross  track 
error  (YLCASE)  vs  time,  the  heading  W  -  a  (HEAD)  vs  time,  the  integral  of  the  cross 
track  error  (YINTGR)  vs  time  and  the  sliding  surface  a  (SS2)  vs=time.  Ir  can  be  seen 
that  the  vehicle  achieves’ the  desired  track  with  no  error.  Figures  3  and  4  show  how  the 
controller  works  with  a  current  and  how  the  larger  the  k^,  the  smaller  the  steady  state 

cross  track  error.  Figure  4  and  (3.12)  also  show  that  as  gets  infinitely  large,  a  steady 

state  error  will  still  exist  and  the  rudder  will  be  cycled  excessively.  The  current  was 
=  0.0,  =  2.0  ft/sec,  which  means  that  =  1.4142  ft/sec,  and  using  (3.14), 

k^^j  =  0.2274.  It  can  be  seen  that  the  steady  state  error,  as  predicted  by  (3.12),  is  in 

accordance  with  what  was  obtained  through  the  numerical  simulations.  Finally,  Figure 
5  shows  that  for  k^  <  k^^^,  the  controller  cannot  guarantee  stability  and  the  controller 

Stans  to  deviate  in  a  linear  manner  in  the  presence  of  a  current. 
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Figure  2.  Cross  Track  Error  Control  with  k  =  0.5  and  F  =0 
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Figure  5.  Cross  Track  Error  Control  with  =  0.2  and  =  2. 
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Results  for  multiple  way  points  are  presented  in  Figures  6  through  9.  The  way 
points  were  selected  at  (X.Y)  =  (10,0),  (20,5)  and  (30,5).  This  corresponds  to  a  lane 
changing  maneuver,  the  change  from  the  original  track  to^a  second  parallel  track.  The 
local  coordinate  system  issrotated  every  time  a  way  point  is  reached.  The  criterion  for 
reaching  a  way  point  is  based  on  the  distance  from  the  way  point  along  the  local  x 
direction,  or  target  distance.  Results  for  various  values  of  the  target  distance  are  shown 
in  Figures  6,  7  and  8,  where  the  target  distances  are  0.5,  2  and  7  vehicle  lengths, 
respectively.  It  can  be  seen  that  if  the  target  distance  is  very  small,  the  vehicle 
overshoots  therdesired  track  with  significant  rudder  activity.  On  the^other  hand,  if  the 
target  distance  is  very  large,  the  vehicle  turns  in  the  wrong.direction  prior  to  completing 
the  turn.  The  best  target  distance  depends  on  the  turn,  vehicle  response  characteristics, 
and  environmental  conditions;  and  in  this  case  it  appears  that  a  value  of  2  causes 
minimal  rudder  and  track  overshoot. 

Finally,  the  attempted  lane  changing  maneuver  in  a  current  =  2.0  is  shown  in 

Figure  9,  where  the  existence  of  a  significant  steady  state  track  error  is  evident.  The 
following  chapters  will  explore  the  use  of  an  integral  control  method  and  the  use  of  a 
disturbance  estimation  and  compensation  method  to  control  the  steady  state  error  in  the 
presence  of  a  current,  such  that  the  vehicle  remains  on  the  desired  track.  These  methods 
of  control  will  utilize  the  above  development  of  the  desired  track.  Once  the  desired  track 
has  been  defined,  these  methods  of  control  will  attempt  to  control  the  cross  track  distance 
to  zero,  so  that  the  AUV  will  be  on  the  desired  track  as  consistently  as  possible. 


32 


iiriTGF  ISHJP  LENGTH5J  iLCR5E  ISHF"  LEIIGTri'; 


X  (SHIP  LENGTHS  I 


0.0  OO.O  i.iO.0  n.O  60.0 

i  lIC  i5ECi  THIE  i 

Figure  6.  Lane  Changing  Maneuver,  Target  Distance  0.5  Vehicle  Lengths. 


Figure  7.  Lane  Changing  Maneuver,  Target  Distance  2  Vehicle  Lengths. 


SHIP  LENGTHS)  YLCRSE  (SHIP  LENGTHS'  i  (SHIP  LENGlHSi 


X  (SNIP  LENGTHS' 


TTME  I  SEC  I 


TIME  (SEi::i 


O 


TIME  (SEC I 


Figure  9.  Lane  Changing  Maneuver  in  a  Current. 
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IV.  INTEGRAL  CONTROL  METHOD 

A.  INTRODUCTION 

In  the  previous  two  chapters,  the  sliding  mode  control  development,  along  with  the 
nominal  straight  line  track  and  the  steady  state  error  in  the  presence  of  a  current,  for  a 
cross  track  error  controller,  have  been  developed.  The  first  method  to  be  used  to 
eliminate  this  steady  state  error  is  the  integral  control  method.  This  is  the  first  logical 
choice,  since  traditionally,  integrators  have  been  used  to  eliminate  steady  state  errors. 
However,  in  generail,  as  more  integrators  are  added  to  the  system,  then  the  chances 
increase  for  the  system  to  becorne  unstable  due  to  the  poles  being  added  to  the  system. 
Also,  when  the  integral  action  is  introduced,  the  linearized  equations  for  the  system  have 
to  be  modified,  which  wilfbe  seen  in  this  chapter.  This  chapter  will  also  show  the  effects 
of  adding  integral  control  to  eliminate  the  steady  state  error  for  a  single  way  point  and 
for  multiple  way  points,  as  in  Chapter  Three.  A  modified  integral  control  rhethod  will 
also  be  investigated  in  this  chapter  with  results  to  show  how  well  the  modified  method 
works. 

B.  INTEGRAL  CONTROL  METHOD 

Before  proceeding  with  the  method  of  integral  control,  the  linearized  system 
equations  must  be  modified.  If  the  cross  track  error  y  needs  to  reach  zero  at  steady  state 
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conditions  insthe  presence  of  constant  disturbances,  thenithe  state  equations  are  augmented 
by 

y;  =  y  .  (4.1) 

Feedback  of  then  brings  in  the  desired  integral  action.  The  augmented  linear  control 

law  becomes 


5  =  +  -k^  +  k/  +  k^  +  k^satsgn{o)  , 


Then  at  steady  state, 


o  =  ^,\j/  +  s^v  +  s/  +  +  S^j 


(4.2) 

(4.3) 


and  (3.4)  still  holds  with  the  additional  y  =  0  from  (4.1). 


C.  STEADY  STATE  ERROR 


The  requirement  of  5^  =  0  and  (4.2)  with  (3.4)'yield 


-1  <  satsgnio)  =  _L  sin"’ 
k. 


<  1  . 


(4.4) 


which  establishes  the  lowest  limit,  k  >  k  . ,  with  k  .  given  by  (3.14).  As  long  as  this 


inequality  is  satisfied,  then  the  integral  control  method  will  drive  the  cross  track  offset 
y  to  zero.  The  closed  loop  poles  on  the  sliding  plane  were  selected  at 
[-0.35,-0.36,-0.37,-0.05],  with  ^  =  0.5  and  u  =  6  ft/sec.  This  gives 
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5  =  1.2948\|/  -  0.0834V  +  1.6206r  +  0.0080>’  +  k^satsgn{G)  (4.5) 

a  =  3.3118\}r  +  0.2635v  +  3.5612r  +  0.0948;y  +  0.0035}'/  .  (4.6) 

Figure  10  shows  how  the  integral  control  method  works  in  the  presence  of  a  constant 
disturbance,  a  current.  The  current  was  U],  =  0.0/,  =  2.0  ft/sec,  which  means  that 

=  1.4142  ft/sec  for  the  chosen  way  point  of  (X,Y)  =  (20,20)  ship  lengths.  Using 

(3.14),  =  0.3081.  Figure  10  was  conducted  using  =  2.0,  which  is  larger  than 


k.,.  From  (4.4), 


-.j 

—  sm  ' 


=  0.1540  , 


thus  the  inequality  of  (4.4)  is  satisfied;  and  as  seen  in  Figure  10,  the  integral  control 
method  drives  the  cross  track  offset  y  to  zero,  with  some  overshoot.  As  k^  is  increased, 

the  cross  track  error  is  brought  to  zero  quicker,  but  the  rudder  is  cycled  much  more 
excessively.  Figure  11  was  conducted  with  =  0.2,  which  is  smaller  than  k^^^,  and  the 


same  current  conditions  as  in  Figure  10.  From  (4.4), 


i.  sin"' 


=  1.5404  , 


thus  the  inequality  of  (4.4)  is  not  satisfied  and  a  steady  state  error  developed.  As  seen 
in  Figure  11,  if  the  nonlinear  gain  isrnot  selected  large  enough;  i.e.,  if  k  <  k  then  the 


integral  controller  cannot  guarantee  zero  steady  state  error.  In  this  case, 
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Figure  11.  Integral  Control  with  =  2  and  for 


satsgn(c)  =  1  , 

which  means  (j  >  (|),  and  using  5^  =  0  and  (3:4), 


Equation  (4.7)^yields  the  steady^state  cross  track  error  of  the  integral  controlleribr  small 
k^.  This  was  seen  in  Figure  IT,  where  the  integral  contror design  developed  a-constant 

track  deviation,  unlike  the  cross^track  error  controller  which  was  unstable.  Using  (4.7), 
with  the  given  values,  =  0.7765,  as  seenTn^the  YLCASE  vs  TIME  graph  of  Figure 

11.  This  unique  characteristic  of  the  sliding  mode  track  controller  -  the  existence  of  a 
■nonzero  steady  state  error  -  is  attributed  to  the  lack  of  a  line^  feedback  gain,  in  y,  in 

(4.2).  The  term  y,  appears  only4n  the  sliding  surface  equatiQn-.(4:3),  and  if  the  nonlinear 

gain  does  not  possess  the  necessary  strength,.it  cannot  guarantee  steady  state  accuracy. 

A  modified  integral  control  method  will  be  developed  later  to  solve  this  problem  with  the 
integral  control:  method.  In  the  case  where  the  integral  controller  is  operating  in  the 
environment  with  no  current.  Figure  12  shows  that  the  vehicle  achieves  the  desired  track 
with  zero  steady  state  error. 

Results  for  multiple  way  joints  are  presented  in  Figures  13  and  14.  The  way  points 
were  again  selected  at  (X,Y)  =  (10,0),  (20,5)  and  (30,5),  for  comparison.  The  target 
distance  for  both  figures  was  two  vehicle  lengths.  Figure  13  shows  the  integral  controller 
not  having  enough  time,  for  the  given  current,  to  drive  the  vehicle  onto  the  desired  trcrck. 
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Figure  12.  Integral  Control  with  No  Current. 
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Figure  13.  Lane  Changing  Maneuver  Under  Integral  Control  in  a  Current, 
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Figure  14.  Lane  Changing  Maneuver  Under  Integral  Control  with  No  Current 


Thecuirentconditions  forFigures  13  and  14wereasin  Figurc  lOwith  =  2.0.  Figure 
14  shows  how  well  the  integral  controller  worksffor  multiple  way  points  with  no  current. 

D.  MODIFIED  INTEGRAL  CONTROL 

An  alternate  design  procedure  that  can  eliminate  the  problem  of  the  existence  of  a 
nonzeroisteady  state  tock  error,  using  iritegral  control,  is  the  modified  integral  control. 
Consider  the  linear  system 


and  the  slidingisurface 


X  =  Ax  +  bu 


(4.8) 


<5  -  S^X  . 

The  sliding  condition  odr  <  0  is  met  by 


(4.9) 


<5  -  -  Ti^^ig/i(a)  ,  (4.10) 

which  gives  the  control  law 

u  =  -{s^by^s^Ax  -  T\\s^by^si8n(c)  .  (4.11) 

Then,  s  can  be  found  as  a  left  eigenvector  of  the  closed  loop  dynamics  matrix  which 
corresponds  to  the  zero  eigenvalue,  as  developed  in  Chapter  Two.  If,  instead  of  (4.10), 

it  is  required  that  ad  <  0  be  met  by 


d  +  ^a  =  -T|^j/g;2(a)  ;  4  >  0  ,  (4.12) 

then  the  control  law  becomes 
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(4.13) 


«  =  -  is^by^s^(A  +  -  Vi\s^by^sign{a)  , 

and  s  can  be  found  as  a  left  eigenvector  of  the  closed  loop  dynamics  matrix  which 

correspondssto  the  eigenvalue  Provided  ^  is  chosen  smalbenough,  (4.12)  satisfies 

a  "near"  sliding  condition  and  a  sliding  condition  in  the  limit,  t  ©o.  In  this  case  of 

track  control,  (4.13)  becomes 

5  =  (/:,+^,)v+(A'2+^j)v+(k3+453)r+(k,+^^)>’+^jy^ 

+k^sai5gn(<5)  .  (4.14) 

Results  are  presented  in  Figure  15  for  =  1.4142  ft/sec  lateral  current,  £  =  0.1  and 

=  0.2.  It  can  be  seen  that  the  presence  of  the  %  -  term  in  (4.14)  eliminates  the  steady 

state  error  that  is  otherwise  present.  For  values  higher  than  the  response 

characteristics  of  the  two  integral  control  laws  (4.2)  and  (4.14)  are  ver>'  similar,  as  seen 
in  Figure  16.  Figure  16  used  k^  =  2.0  and  when  compared  to  Figure  15  and  Figure  10, 

all  figures  show  similar  results  for  k^  >  k^^^. 
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V.  DISTURBANCE  ESTIMATION  AND;  COMPENSATION  METHOD 

A.  INTRODUCTION 

The  integral  control  methods  of  the  previous  section  will  ensure  zero  steady  state 
error,  for  and  for  k^  <  k^^,,  especially  when  using  the  modified  integral  control 

method.  To  improve  on  the  transient  response  and  to  achieve  the  desired  steady  state,  a 
disturbance  estimation  and  compensation  can  be  introduced  in  the;cross  track  error  design 
of  the  confroller.  This  chapter  will  investigate  the  disturbance  estimation  and 
compensation  method,  which  formulates  the  cuitent  as  a  disturbance  to  be  included 
directly  into^the  control  law.  In  this  chapter,  the  disturbance  compensation  method  will 
first  be  developed  with  a  perfect  current  input  and  then-the  disturbance  estimation  and 
compensation  method  will  be  developed  with  an  estimate  of  the  current  using  a  current 
observer.  This  method  will  follow  the  same  development  as  for  the  integral  control. 
First,  a  single  way  point  will  be  investigated  with  current  and  then  without  current,  to  see 
how  well  both  methods  control  the  AUV  onto  the  desired  track.  Multiple  way  points  will 
next  be  investigated;  and  results  will  be  given  to  show  how  well  both  methods  handle 
these  multiple  way  points.  This  inethodology  will  be  followed  for  the  perfect  current 
input  as  well  as  for  the  estimated  current  input. 
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B.  DISTURBANCE  COMPENSATION  METHOD 


The  sli^ng  surface  (3.6)  is  modified  to 


a  =  5,\i/+52V+.S3r+.s^+  (})— +5,  sin"’  _f 


with  5  as  in  (3.5).  At  steady  state,  r  =  =  5^  =  0  and  (3.4)  is  valid  with  y^  =  0,  as 

dictated  by  (5.1)  and 


satsgnio)  =  —  . 

If  <  then  the  disturbance  compensation  controller,  with  the  cross  track  error, 
cannot  guarantee  stability.  The  steady  state  response  in  such  a  case  is  characterized  by 


r.  =  V  =  5  =  0 


s  s  s 


and  by 


•  1 

V,  =  -sin  ’  ^  , 


with  linearly  increasing  in  time  with  the  rate  of  change  given  by 


''c  • 

y  =  ^  -  sm  _1  . 

"  u  k. 


Equation  (5.3)  is  obtained  from  (3.5)  for  5=0  with  satsgn(<5)  =  1 , 
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C.  STEADY  STATE  ERROR 

Again  the  requirement  of  6^  =  0  andv(4.2)  with  (3.4)  yields  (4.4),  which  establishes 

the  lowest  limit,  with  k-^^  givemby  (3.14).  Again,  as  long  as  this  inequality 

is  satisfied,  the  disturbance  compensation  method  will  drive  the  cross  track  offset  y  to 
zero.  The  closed  loop  poles  on  the  slidingjplane  were  again  selected  at 
[-0.35,  -0.36,  -0.37],  with  (j)  =  0.5  and  u  =  6  ft/sec.  This  gives 


5  =  0:9556\i/  -  0.1085V  +  1.2286r  +  k^satsgn{<5) 


(5.5) 


<5  =  2.9805v+0.2199v+3.4445r+0.0700y+ 


0.4778 


+2.9805 


sih-'(0.2357).  (5.6) 


J 


Figure  17  shows  how  the  disturbance  compensation  method  works  iii  the  presence  of  a 
current,.  =  0.0,  =  2.0  ft/sec,  which  means  that  =  1.4142  ft/sec  for  the  chosen 

way  point  (X,Y)  =  (20,20)  ship  lengths.  Using  (3.14),  k^^^  =  0.3081 ,  as  for  the  integral 


control  method.  Figure  17  was  conducted  using  a  perfect  current  input  and  k  =  2.0, 


which  is  larger  than  k^^^.  As  seen  in  Figure  17,  the  disturbance  compensation  method 

brings  the  steady  state  error  to  zero  with  no  overshoot  and  with  a  quicker  response  than 
the  integral  control  method.  In  Figure  17  and  all  subsequent  similar  figures  for  the 
disturbance  compensation  and  the  disturbance  estimation  and  compensation  methods,  the 
following  variables  are  displayed:  X  vs  Y  position,  rudder  angle  vs  TIME,  the  cross  track 
error  (YLCASE)  vs  TIME,  the  heading  v  -  a  (HEAD)  vs  TIME,  (VCUR)  vs  TIME 
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Figure  17.  Disturbance  Compensation,  =  2,  k^> 
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and  (tJCUR)  vs  TIME,  for  the  perfect  currenginput;  and  v;.  (VCOOBS)’vs  TIME  and 

(UCOOBS)  vs  TIME,  for  the  estimated  current  input.  ^Figure  18  shows  how  the 

disturbance  compensation  method  works  in  thetpresence  of  the  same  current  conditions 
as  in  Figure  17  for  =  0.2,  which  is  less  than  As  seen  in  Figure  18,  the 

disturbance  compensation  methodidesign  givesmse  to  unstable  behavior. 

Results  for  multiple  way  points  are  presented  in  Figure  19.  The  way  points  were 
selected:at  (X,Y)  =  (10,0),  (20,5)iand  (30,5)  ship  Iengths,,for  comparison.  Again,  the 
target  distance  was  selected  at  2  vehicle  lengths.  Figure  19  was  conducted  with  the  same 
current  conditions  as  tin  Figure  17. 

D.  CURRENT  OBSERVER  DEVELOPMENT 

As'seen  in  the  previous  section,  the  disturbance  compensation  method  works  well 
with  absolute  knowledge  of  the  current.  However,  in  reality,  the  current  is  never 
absolutely  known  at  every  location,  so  a  current  pbserver  must  be  developed.  A  reduced' 
order  observer  can  be  designed  based  on  y,  \|/  and  r  measurements  to  estinhate  the  lateral 
current  velocity  and  the  current  velocity  along  the  track  u^.  The  observer  design  is 
based  on-  the  linear  equations  (2.1a),  (2.1b),  (2.1c)  and 

5’  =  +  MV  +  V  (5.7) 
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Figure  18.  Disturbance  Compensation,  V  =  2,  k  <  k  . 
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Figure  19.  Lane  Changing  Maneuver  Under  Disturbance  Compensation  in  a  Current. 
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X 


u.  + 


V 


e 


=  0 


=  0  . 

Rewriting  these  equations  intoimatrix  form, 


d 

^1 

4,1  4,2 

% 

(x] 

■’■I 

K 

=  - 

4- 

dt 

4^1  4^2^ 

?2. 

where 

x/=[\}rr  3»x]  , 
xj  =  fv  wj  , 


0  10  0 
0  fljjM  0  0 

10  0  0 
0  0  0  0 


(5.10) 


(5.11) 
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0  0  ol: 


A 


12 


fljiM  0  0 

T  1  0 
0  0  1 


A 


21 


0  0  0 

0  0  0  0, 

0  0  0  0 


A 


22 


a^^u  0  0 
0  0  0. 
0  0  0 


=  [0, 0, 0]  , 

and 

0,  0]  . 

Equation  (5. 11)  takes  on  the  state  space  form  of 


X  =  Ax  +  Bh 


(5.12a) 


y  -  Cx 


(5.12b) 


where 


C  =  /  . 


are  the  measurable  states  and  xj  arc  the  states  to  be  estimated  or  observed. 


Expanding  (5.12a), 
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X,  =  y4„X,  +  +  fi,5 

Xj  =  v42,x,  +  t  . 

From  the  buenberger  reduced  order  observer  development  for  Xj,  the  estimated  or 
observed  states  are 

Xj  =  bx,  +  z,  ,  (5.13) 


and 


z  =  Ft  +  Gx,  +  Hh  ,  (5.14) 

where 


and 


II 

-  L/l,j  , 

(5.15) 

C  =/i„ 

(5.16) 

H  =  LB^  .  (5.17) 

In  the  above  equations,  the  L  matrix  needs.to  be  determined.  The  MATRIX_x  software 
package  is  unable  to  determine  the  L  matrix  directly  because  more  than  one  output  is 
measitrable.  Therefore,  the  L  matrix  will  be  determined  manually.  Let 


^2 

L  = 

^23 

4. 

^3. 

^32 

^3. 
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and  choose  everything  zero,  except  so-that 


From  (5.15), 


0  /,2  0  0 
0  0  Ijy  0 

0  0  0  /„ 


F  =  A^- 


^23  “^23  ^ 

0  0  -ly. 


(5.18) 


Now  choose-the  observer  poles  of  and  jj  to  be  at  -1.0,  -1.1  and  -1.2.irespectiveiy, 


which  are  at  least  two  times  faster  than  the  controller  poles,  defined  in  the  above  section, 
as  . required  by  a  good  observer  design.  Placing  the  observer  poles  in  mauix  form  and 
equating  to  (5.18), 


s,  =  -  /, 2^2,1/  , 


^2  “  ^23 


(5.19a) 

(5.19b) 


and 


^3  -/j^  . 


(5.19c) 


Solving  (5.19)  for  /,j,  and  /j.. 
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,  _  -  -^1 
*12  “  _  » 

V 

(5.20a) 

^23  “  ~^2 

(5.20b) 

and 

/„  =  -^3  .  (5.20c) 

From  (5:20),  the  L  matrix  is  now  determined,  therefore,  F,  G  and  H  are  also  determined. 

From  (5114),  the  reduced  order  observer  equations  become 

z,  =  j,2,+(a,2U-/,jflj2U+j,/,2)r+(i»,«2_/^^£,^„2j5 

(5.21a) 

z^  =  SjZ,+5^Zj-/23Msin(\j/-a)+jj/j3y+y,,r 

(5.21b) 

z,  =  . 

(5.21c) 

From  (5.13),  the  equations  for  the  wtimated  or  observed  quantities  become 


V  =  z,  +  /,jr  ,  (5.22a) 

=  z,  +  /jjy  (5.22b) 

and 

u^  =  z^  +  lyX  .  (5.22c) 

Due  to  the  way  wm  defined  in  (5.8), 
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(5.23) 


=  ^  MCos(Y  -  a) 


and 


0,^  -  .  (5.24) 

The  sine=and  cosine  temis  are  not  linearized  in  (5.21b)  and  (5.23)  to  eliminate  steady  state 
errors  in?the  observer  when  the  angle  y  -  a  becomes  significant  ati  steady  state  in  the 
presence  of  strong  currents.  Since  thefcurrent  perpendicular  to  the  track  and  the  current 
along  thedine  of  each  frack  will  be  different  locally  every  time  the;  AUV  drives  onto  a 
new  track,  the  current  observer  quantities  of  and  Zj  need  to  be  reset  every  time  a  new 

way  point  is  called  for  by  the  autopilot.  The  quantities  z^  and  Zj  are  used  to  determine 
r..nd;  «'  which  determine  and 4,  .  The  equations  used  tomeset  z,,  z.,  ■0  and 

‘oh,  ‘oil  j-  .  iic 

J 

Uc  are 


and 


=  v^cosa  -  tioSina  , 

(5.25) 

=  #cosa  -  V  sina  , 

(5.26) 

Zz  =  V  -  l  Ay  , 

(5.27) 
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Z3  =  -  l.2y  .  (5.28) 

These  equations  are  used  as  the  new  way  point  is  asked  for  and  prior  to  entering  the 


observer  for  the  first  time  on  the  new  track. 


E.  DISTURBANCE  ESTIMATION  AND  COMPENSATION  METHOD 

With  the  development  of  a  current  observer,  reality  can  now  besbettef  incoqiorated 
into  the  controller  design.  The  sliding  surface  (5.1)  is  now  modified  to 


r 


a  =  s^)]r+S2V+s/■i^s^+ 


#1 


sin 


V 


(5.29) 


with  5  as -in  (3?5).  The  rest  of  the  development  for  the  disturbance  estimation  and 
•compensation  method  is  exactly  the  same  as  for  the  disturbance  compensation  method. 
Figure  20  shows  how  well  the  disturbance  estimation  and  compensation  method  work.s 
in  the  presence  of  the  same  environmental  conditions  as  in  Figure  17.  Also  the  response 
of  Figure  20  is  virtually  the -same  as  for  the  disturbance  compensation  method  in  Figure 


T7.  Figure.20  used  a  k  =  2.0,  which  is  larger  than  Again  for  k^  =  0.2,  which  is 


less  than  k^^^,  unstable.behavior  results,  as  seen  in  Figure  21.  Figure  22  shows  the  resuhs 

of  the  disturbance  estimation  and  compensation  method  with  no  current. 

Results  for  multiple  way  points,  using  the  disturbance  estimation  and  compensaiion 
method,  are  presented  tin  Figures  23  and  24,  with  a  target  distance  of  2  vehicle  lengths. 
Figure  23  was  conducted  using  the  same  current  as  in  Figure  17.  It  can  be  seen  that  the 
disturbance  estimation^and  compensation  method  works  as  well  as  the  disturbance 
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Figure  23.  Lane  Changing  Maneuver  Under  Disturbance  Esiimatiomand 
Compensation  in  a  Current. 
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compensation  method,  with  perfect  current  input.  Figure  24  shows  how  well  the 
disturbance  estimation  and  compensation  method  works  for  multiple  way  points  with  no 
current.  In  Figures  21  and  23  for  zero  estimated.cunent  in  the  track,  it  can  be  seen  that 
there  is  a  small  nonzero  current  in  the  graph  of  UGOOBS  vs  TIME.  This  small  nonzero 
current  is  a  result  ofsthe  integration;time  step  not  being  small  enough.  Figures  20  through 
24  were  alkrun  at  atO.Ol  second  time  step.  As  the  time  step  decreases,  the  time  to  run 
the  programrincreased  dramatically. 

F.  MODIFIED  DISTURBANCE  ESTIMATION  AND  COMPENSATION 

In  order  to  overcome  the  instability  of  ihe  disturbance  estimation  and  compensation 
method  for  a  mcxiified  design  will  be  considered.  Similarly  to  the  integrals 

control  method  of  Chapter  IV,  condition  (4.12)  will  be  required  to- be  satisfied  instead  of 
(4.10),  and  the  control  law  then  becomes 


5  =  ik^+^^)^\f^■{k2^■^s^)v+{k^+^J)r+^J■i■k^satsgnio)  , 


(5.30) 


a  =  5j\|/+^jV+j'3r+j'^+ 
Thenj  at  steady  state  =  0  provided 

k.  >  kj.  = 


- - +5, 


sm" 


H  cm 


J 


f  \ 


(  \ 
V 


V  “  y 


(5.31) 


(5.32) 


(/:,  +  ^,)  sin'* 

In  this  case,  I  can  be  found  as  the  left  eigenvector  of  the  closed  loop  dynamics  matrix 
that  corresponds  to  the  eigenvalue  For  small  values  of  the  same  s  and  k  can  be 
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used  as  before.  If  <  k^J,  this  modified  design  develops  a  finite;steadv  state  error,  y^, 


computed  frorri  5^  =  0  and  >4-  Results  are  presented  in  Figure  25  for  the  same 

conditions  asdn  Figure  21,  with  =  0.5.  It  can  be  seen  that  the  nonzero  value  of  4 
stabilized  the  vehicle  and  reduced  the  path  error.  This.  hpwever,  is  mot  always  the  case. 
The  explanation  lies  in  the  fact  that  nonzero  values  of  ^  raise  theicritical  as  sliown 

in  (5.32).  The  steady  state  cross  track  errors>versus  ^  and  k^  areishown  in, perspective 

views  in  Figures  26,  27  Md  28,  form  =  6  ft/sec  and  =  1, 2.5  and4  ft/sec,  respectively. 

It  can  be  seen  that  for  k^  <  is  reduced  by  increasing  ^.  although  beyond  a 

certain  point  the  corresponding  reducdon  in  y^  is  insignificant.  For  k^  >  the  value 

of  4  should  hot  be  increased  beyond  the  value  that  renders  k^  =  in  (5.32),  unless  the 

vehicle  is  expected  to  operate  in  high  current  environments  that  would  increase  the  value 
of  in  (3.14). 

Chapters  3,  4  and  5  have  dealt  with  methods  to  control  the  cross  track  •error,  due 
to  a  constant  disturbancej  to  zero.  Now  that  the  methods  have  been  developed  and  shown 
to  work  well,  the  next  chapter  will  devise  a  technique  to  help  optimize  the  time  to  turn, 
so  that  the  turn  is  initiated  and  conducted  in  the  most  efficient  manner  given  any 
environmental  conditions.  This  technique  will  be  referred  to  as  the  jeading  track  control 
monitoring  technique. 
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VI.  LEADING  TRACK  CONTROL  MONITORING  TECHNIQUE 

A.  INTRODUCTION 

Methods  forcontroIHhg  a  cross  track  error,  due  to  a  constant  disturbance,  ha%'e  been 
developed  in  Chapters  3, 4;and  5.  In  each  method,  it  was  seen  that  the;desired  track  was 
attained  within  the  limits  of  the  theory',  with  the  appropriate  values  ot  It  was 

also  seen  that,  for  multiple  way  points,  the  target  distance  played  a  major  role  on  how 
well  the  AUV  initiated  theuum  so  as  to  attain  the  next  track.  The  ability  of  the  .AUV  to 
turn  depends  on  the  environmental  conditions,  the  vehicle  response  characteristics  and  the 
turning  angle,  as  discussed  in  Chapter  III.  This  chapter  will  develop  a  technique  that 
monitors  the  leading  trackf  in  order  to  determine  the  correct  time  for  the  AUV  to  initiate 
the  turn  with  no’ovcrshoot  and  minimal  rudder  use.  This  technique  is  referred  to  as 
^leading  q-ack  control  monitoring. 

The  concept  of  leading  track  control  trioniioring  is  to  use  two  legs,  the  current  leg 
to  control  the  cross  track  error,  or  track  deviation;  and  the  second  leg,  to  control  course 
deviation,  or  to  determine  the  correct  time  to  initiateahe  turn  onto  the  new  track  [Kef. 
16j.  This  chapter  will  also  show  results  on  how  weh  the  leading  track  control  monitoring 
technique  works  as  compaued  to  using  the  previous  control  methods;  i.c.,  disturbance 
compensation,  disturbance  estimation  and  compensation  and  integral  control;  with  various 
target  distances. 
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B.  LEADING  TRACK  CONTROL  MONITORING  DEVELOPMENT 

A  simple  technique  to  automatically  initiate  the  turn  onto  the  new  track  at  the 
proper  time  will  be  used  for  thefcpurse  change  from  the  current  track  to  the  leading  track. 
The  vehicle  is  assumed  originally  to  be  sailing  on  the  cunent  track.  Now  consider  the 
application  of  the  control  law  for-the  leading  track,  simultaneously,  which  will  order  the 
rudder  command  to  drive  the  vehicle  onto  that  track.  Two  control  laws  are  constructed: 
one  control  law  for  the  current  frack,  which  is  used  to  reduce  the  track  deviation;  and  one 
control  law  for  the  leading  track,  which  is  used  to  monitor  course  deviation.  To  make 
a  smooth  connection  from  the^curfent  track  to  the  leading  track,  the  control  law  for- the 
leading  track  will  be  monitoredin  addition  to  the  present  control  law  for  the  current  track, 
simultaneously.  In  the  beginning,  the  track  deviation  is  much  larger  than  the  course 
deviation.  However,  in  the  mean  time,  the  track  deviation  will  be  decreased  and  the 
course  deviation  will  become  dominant.  Therefore,  a  smooth  connection  can  be  attained 
by  switching  the  actual  control  from  the  current  track  to  the  leading  track  as  soon  as  the 
mof  itored  leading  track  control  reaches  zero.  The  leading  track  control  that  reaches  zero 
is  the  point  the  rudder  for  the  leading  track  changes  sign  from  positive  to  negative  or  vice 
versa. 

C.  RESULTS 

Figures  29  through  34  show  the  results  of  the  leading  track  control  monitoring 
technique,  which  automatically  determines  the  point  to  initiate  the  turn  onto  the  next 
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Figure  29.  Leading  Track  Monitoring  Technique:  5°  Course  Change, 
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Figure  30.  5°  Course  Change:  Fixed  Target  Distances  of  0.5,  2  and  4  Vehicle 

■Lengths. 
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Figure  32.  45°  Course  Change:  Fixed  Target  Distances  of  0.5,  2  and  4  Vehicle 
Lengths. 
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track,  applied  to  the  disturbance  estimation  andlcompensation  method- and  compared  to 
fixe^t^get  distances  of;0^^5,:2  and  4  vehicledengths,  respectively,  formo  current.  Figures 
29,: 31  and  33  show  the  results  of  the  leading  track-control  monitoringitechnique,  with  the 
disturbance  estimation  andjcompensation  method,;for  course  changes  of  5°,  45°  and  90° , 
respectively.  The  leading;track  monitored  rudderjangle  and  course  deviation  are  refeired 
to  as  DR99  and  YGTE99  in  the  graphs,  respectively.  The  leading  track  control 
monitoring  technique  can;also  be  used  with  the-other  control  methods  discussed  in  the 
previous  chapters,  howevef,  for  Figures  29  through  34,  the  disturbance  estimation  and 
cornpensation  method  is  utilized.  Figures  30,  32  and  34  show  the  results  of  the 
disturbance  estimaddn  arid'  compensation  method  for  target  distances  of  0.5,  2  and  4 
vehicle  lengths,  from  top  toibottom  respectively.  For  each  course  charige  of  5°,  45°  and 
90°,  there  is  one  target  distance  that  is  best  for  that  course  change^  and  it  will  not 
necessarily  be  the  best  for  the  other  course  changes.  For  example.  Figure  30  shows  that 
a  target  distance  of  2  vehicle  lengths  is  best  for  a  course  change  of  5°.  However,  for  tlic 
course  changes  of  45°  and  90°,  target  distances  of  2  to  3  and  4  vehicle  lengths, 
respectively,  are  best  for  these  course  changes.  The  leading  track  control  monitoring 
technique  eliminates  the  need  to  wony  about  what  target  distance  is  required  because  the 
technique  automatically  determines  the  distance  required  to  initiate  the-tum  onto  the  next 
track  without  any  overshoot  and  minimal  rudderuse. 

Figures  35  through  39  show  the  performance  of  the  leading  track  monitoring  control 
technique  in  the  presence  of  a  cuirent  for  the  multiple  way  points  used  in  the  previous 
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Figure  35.  Lane  Changing  Maneuver,  Disturbance  Estimation  and  Compensation, 
Leading  Track  Monitoring  Technique:  Current  =  0,  =  2, 

Course  Change  5  :  10. 
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Figure  37.  Same  As  Figure  35  with  Course  Change  5  :  5. 
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Figure  39.  Lane  Changing  Maneuver,  Disturbance  Estimation  and  Compensation, 
Leading  Track  Monitoring  Technique:  Cuirent  =  -2.5, 

Course  Change  5  :  10. 
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chapters.  EigUres  35  through  37  were  conducted  at  =  0.0  and  =  2.0  ft/sec  for 

increasing  course  changes.  It  can  be  seen  that  the  leading  track  control  monitoring 
technique  works  superbly  in  theipnesence  of  a  current  for  increasing  course  changes  with 
no  overshoot  for  each  turn  onto  the  leading  track,  with  miniirial  use  of  the  rudder. 
Figures  38  and39  show  the  leading  track  controf  monitoring  techiiique  for  currents  other 
than  that  used  <in  Figures  35  through  37.  Figure  38  used  a  current  of  =  2.0 

ft/sec  and  Figure  39  used  a  current  of  =  -2.5  fi/scc.  Both  of  these  figures 

reveal  that  the  leading  track  control  monitoring  technique  can  handle  very  large  currents, 
within  the  physical  constaints  of  the  vehicle,  very  well. 

The  automatically  selected  target  distance  d^in  vehicle  lengths),  by  this  technique, 
is  plotted  in  Figure  41  for  different  current  magnitudes  and  directions  (the  symbols  are 
explained  in  Figure  40)  and  u  =  6  fi/sec.  It  can  be  seen  that  d  depends  on  both  the 
strength  v  and  orientation  9  of  the  current  and  the  turning  angle  a.  As  the  angle  a  is 
increased,  d  is  also  increased,  as  expected.  The  same  is  true  for  increasing  current  speed. 
For  very  small  changes  in  vehicle  path  a,  the  leading  track  control  monitoring  technique 
tends  to  be  conservative;  i.e.,  it  initiates  the  turn  early  with  very  little  rudder  usage.  If 
the  technique  is  modified  such  that  the  actual  switching  occurs  when  the  monitored  rudder 
angle  reaches  a  specified  value  (such  as  its  saturation  limit)  after  the  zero  crossing,  then 
smaller  target  distances  can  be  achieved  for  small  a.  Also,  the  technique  in  its  current 
application,  cannot  handle  turning  angles  more  than  90®.  Although  such  turns  arc  rarely 
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d  target  distance  (automatically.selected) 

a  course  ch&ge  (comrnanded  turning  angle) 

0  current  direction 

V  current  resultant  speed 


Figure  40.  Nomenclature  for  Leading  Track  Monitoring  Technique. 


Figure  41.  Automatically  Selected  Target  Distances  for  Different  Gurrcnt 
Magnitudes:  aiid  Directions. 


demanded  for  by  .the  path  iplanner,  ;the  technique  can.  be  modified  to  allow  for.  these 
drastic  turns  if  desired. 

In  the  finalichapter,  noise  willibe  introduced  intmthe  measureable .parameters;  i.e., 
\j/,  r,  y  of  x;  and^the  effects  will  be  evaluated.  Also,, guidelines  will  be  developed  for 
suppressing  the  effects  of  measurement  noise. 
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VII.  ROBUSTNESS  TESTS  /^ND  SENSOR  NOISE  EEFECTS 

A.  INTRODUCTION 

The  characteristics  of  the  controllers  that  wererdesigned  in  the  previous  chapters  are 
.analyzed  here  with  a  view  to  their  robustness  properties  with  respect  to  unmodelled 
dynamics  and  actual/mathematical  moder  mismatch.  For  the  sake  of  brevity,  emphasis 
is  placed  on  theulisturbance  estimation  and  compensation  design.  The  effects  of  sensor 
noise  ^d  sensor  drift  are  also  evaluated  through,  a  series  of  digital  simulations.  This 
^brings  another  l&vel  of  realism  into  the  design. 

B.  ROBUSTNESS  PROPERTIES 

The  effect  of  the  sway  velocity  pbsen'er  is  evaluated  in  Figure  42.  Curve  1-is 
obtained  by  using  the  observed  value  of  th&sway  velocity  v,  . whereas, Curve  2Hs  obtained 
%  assuming  that  V  =  0.  The  vehicle  speed  u  was^kept  constant  at  u  =  6  ft/^ec,  and  the 
'lateral  current  was  =  2  ft/sec.  Disturbance  estimation  and  compensation  was  used 
with  5  and:(|)  =  0.5.  It  can  be  seenithat  the  response  of  the  two  curves  is  almost 

identical;  It  ca^n:  be,  therefore,  concludedithat  the  sway  velocity  does  not  appear  to<be 
very  significant  for  track  control  design.  This  result  is  analogous  to  the  Line  of  Sight 
navigation  case  [Ref.  12], 

Results  for  differentTorwardispeeds  tire  shown  in  Figure  43,  for  the  same  conditions 
as  in  the  previous  test.  Curve  2  was  obtained  for  u  =  6  ft/sec  (nominal  design),  while 
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Figure  42.  Robustness  Test:  Sway  Velocity  Effwu 
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Figure  43.  Robustness  Test:  Forward  Speed  Effect. 
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Curve  1  is  for  u  =  3  ft/sec  and:  Curve  3  is  for  u  =  12  ft/sec,  with  the  same  gains  and; 
slidingiplane  coefficients  as  for-the  noniinal  case.  It  can  be  seen  that  large  deviations  ins 
the  forward  speed  can  be  accomodated  by  the  controller  without  the  need  for  gain 
scheduling. 

The  robustness  of  the  compensator  is  also  evaluated  in  Figure  44  for  a  drastically 
off  design  casefCurve  2).  which  is  shown  along-with  the  response  of  themominal  design; 
(Curve  1).  The  same  current  =  2  ft/sec  is:present.  For  Curve  2,  the  values  of  the 

hydrodynamic  coefficients  and  were  reduced  in  the  equations  of  motion:  to  half  of 

their  actual  values,  andMhe  rudder  coefficients  Tj  and  A/g  were 'increased  to  twice  their 

actual- values.  'Both  ofuhese  changes  correspond  to  a  more  responsive ;and  less  damped' 
vehicle;  The:Cdritrollen and  observer  were  designed  for  the  true  values  ofithe  coefficients, 
so  thauthe  vehicle  is  operating- with  large  errofsnn  the  knowledge  of  itstdynamics.  The 
results  of  Figure  44  demonstrate  the  ability  .of  the  controller  to  meet  its  mission 
requirements  even  under  unrealistic  errors  in  the  design.  The  trapk  overshoot  for  the  off 
design  case  is  attributed  to  the  slower  convergence  of  the  curfent  observer  to  the  true 
currentsspeed,  as  is  also  showritin  Figure  44. 
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C.  EFFECTS  OF  SENSOR  NOISE 

So  far,  incomplete  but  perfect  state  measurement  has  been  assumed.  To  analyzesthe 
effects  of  sensor  noise  on  the  sliding  mode  track  conu-oller  with  the  disturbance 
estimation  and  compensation  method,  the  following  scenario  is  considered.  The  vehicle 
is  moving  with  u  =  6  ft/sec  in  a  lateral  current  =  2  ft/sec.  Controller  poles  are 

selected  at  -0.25  and  observer  poles  at  -0.50.  The  measurable  quantities  are  \}/,  r  and  y, 
and  the  noise  is  simulated  by  Gaussian  disttibution  with  typical  standard  deviations  of^O.  I 
degrees  for  y  ,  0.01  degree.s/sec  for  r  and  0.»1  ft  for-y.  All  simulations  are  perforrried 
using  Euler  iritegrations  with  time  step  At  =  0.1  seconds.  This  corresponds  to  a  sample 
rate  of  10  hertz,  which-is  reasonable.  All  results  show  time  histories  of  the  exact,  not  the 
measured,  lateral  deviation  y,  in  vehicle  lengths,  and  the  actual  rudderangle  5;  in  degrees. 
The  same  scale  has  been  keptTor  alltgraphsTor  comparison. 

The  results  of  the  simulation  for  =  2,  ^  =  0.5  and  for  noise  free  sensors  are 

presented  in  Figure  45.  When  the  assumed  noise  is  introduced,  as  in  figure  46,  the  actual 
y  does  not  differ  significantly.  The  rudder  angle  5,  however,  is  chattering  so  that  th’' 
design  cannot^be  accepted.  If  the  value  of  ^  is  increased  to  5,  then  the  level  of  rudder 
chattering  is  significantly  reduced  at  the  expense  of  a  slower  vehicle  response,  as  shown 
in  Figure  47.  The  level  of  4>  is  ultimately  related  to  the  standard  deviation  of  the  sliding 
plane  a.  If  a  faster  vehicle  response  is  needed,  then  (J>  can  become  a  function  of  c,  so 
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ithat  itis  small  away  from  the  sliding  plane  and  becomes  larger  as  the  system  approaches 
<j  =  0?  Keeping  the  saihe  ^  =  0.5  and  reducing  to  0.5  helps  to  reduce^  the  level  of 

chattering,  as  shown  in  Figure  48.  This,  however,  has  the  cffcct  of  possibly  sacrificing 
stability  or  steady  state  accuracy,  as  analyzed  in  the  previous  chapters.  I:  follows  then, 
that  the  first  action  to  suppress  the  noise  effects  must  be  to  increase  the  value  of  <>  . 

Another  way  to  further  improve  on  the  response,  in  a  noisy  set  of  measurements, 
is  to  introduce  a  first  order  lag  between,  commanded  and  actual  rudder  angle.  If  T, 

denotesnhe  anificial  (software)  steering  gear  time  censtant,  then 

rS  +  5  =  5^  ,  (ll) 

where  5^_is  the  commanded  rudder  angle  and  5  the  actual  nidder  angle.  A  time  constant 

=  0.5  seconds,  which  is  five  times  higher  thari  the  integration  Step,  should  provide 
enough  noise  attenuation,  since  the  comcrTrequcncy  of  (7.1)  is  2,  while  the  frequency  of 
the  noise  is  10.  At  the  same  time  a  value  of  T,,  =  0.5  seconds  is  small  enough  so  that 

the  transient  response  characteristics  of  the  vehicle  are  not  significantly  affected.  The 
results  ^  shown  in  Rgure  49,  and  for  comparison,  the  response  of  the  idemical  system, 
with  noise  free  rensors,  is  shown  in  Figure50.  If  a  faster  rwponsc  is  nccessm',  then  the 
controller  has  tO  be  redesigned  by  taking  (7.1)  as  an  extra  state  equation. 

Very  low  values  of  T,  (of  the  same  order  Of  magnitude  as  At)  do  riot  have  any 

visible  effects  in  noise  r^uction,  while  large  values  of  T-  can  deteriorate  the  iransicni 
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response  characteristics  significantly.  The  latter  is  demonstrated  in  Figure  51  for  T  =  5 
seconds.  Finally,  even  if  4  is  kepbat  0.5,  introduction  of  =  0.5  seconds  reduces  the 

chattering  significantly,  as  seen  inrFigure  52. 

It  follows  that  increasing  the  value  of  ^  and  introducing  an  appropriate  software 
rudder  time  constant  T  are  two  major  guidelines  fonreducingithe  effects  of  sensOr  noise 

and  still  keep  satisfactory  transient  response.  Of  course,  observer  gains  can  be  established; 
'be  a  Kalman  filter  design.  This  should  help  in  minimizing  the  variance  of  the  control 
effort  and  response  evenTurther. 

D.  EFFECTS -OF  SENSOR  DRIFT 

Having  analyzed  the  effects  of  sensor  noise,  a  different  aspect  of  sensor 
imperfection,  namely  sensor  drift,  will  now  be  investigated.  The  most  critical  sensor  drift 
for  the  track  keeping  problem  is  the  offset,  or  positional  drift- of  the  Inertial  Navigation 
System.  Along  with  the  simulated-noise  of.the  previous  section,  the  offset  measurement 
is  assumed  to  experience  a  drift  of  one  vehicle  length  in  ten  dirnensionless  seconds  before 
the  next  exact  navigational  update  comes  up.  For  simulation  purposes  the  drift  is 
assumed'to  be  linear  between  the  two  updates.  Results  for  the  lateral  offset  y  and  rudder 
.angle  5  are  presented  in  Figure  53  for  u  =  6  ft/sec, =  2,  ^  =  5,  T  =  0.5  seconds 

and  lateral  current  =  2  ft/sec.  As  expected,  the  vehicle  drifts  off  the  y  =  0  track 
following  the  sensor  read  but. 


105 


This  lateral  offset. drift,  or  bias^-caiinot  unfortunately  be.  estimated  by  an.obser\'er 
because  the  dynamic  system  is  unobservable  unless?y  is  measuredi  One  way  todmprove 
the  response  is  to  record  the  two  most  receipt  navigational  up^tes  together  with  tlie 
corresponding  sensor  readings.  Them  the  d  nft  is  assumed  to  be  line^.  and  tliis 
result  is  extrapolated  until  the  next  navigation.  ?  fix,  and  the  process  is  repeated.  The 
vehicle  response  is  now.=satisfactory,£as  shown  M  JFigure  54,  unlessJthe  actual  sensor  drift 
is  significantly  different-than  the  extrapolated,  as  is  tl,e  case  between  10  and  20,  and 
30  and  40  dimensionless  seconds. 

E.  NAVIGATIONAL  IJPDATES-EFFECT 

So  far,  knowledge  of  y  is  assunTed  to  occur  atnhe  same  raleras  the  simulation  step, 
or  the. autopilot  updatesdn  y  .and  r.  In  reality,  thii-will  probably  not  be  the  case,  since 
measurement  of  y  is  more  involved  th|n  \|/  or  r,  andrwill  thus  occur  at:  a  slower- rate.  The 
effects  of  updating  the  cross  track  error  at  a  slower  rate  are  analyzed  in  Figures  55 
through  61  for  u  =  6  ft/sec  and  =  2  ft/sec.  The  actual  path,  not  the  one  that  is 

available  to  the  compensator  at  all  times,  is  plotted  versus  time  using  the  same  scale  in 
aU  figures  fw  comparison.  The  response  of  the  nominal  design  is  shown  in  Figure  55  for  =  5 

and  (j)  =  0.5 .  The  response,  when  the  cross  track  error  y  update.^5  are  10, 20  and  30  times 
slower  than  the  integration  step,  is  presented  in  Figures  56  through  58.  It  can  be  seen 
that  in  the  latter  case,  the  vehicle  is  unstable.  It  should  be  pointed  out,  though,  that  it 
should  not  really  be  expected  to  apply  a  cross  track  error  compensator  in  a  strong  current 
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Figure  55.  Navigational  Updates  Effect:  Nominal  Design. 
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Figure  56.  Navigational  Updates  Effect:  Navigational  Updates  10  Times  Slower 
'I'han  Autopilot 
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Figure  57.  Navigational  Updates  Effect:  Navigational  Updates  20  Times  Slower 
Than  Autopilot. 
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Figure  58.  Navigational  Updates  Effect:  Navigational  Up^tes  30  Times  Slower 
Than  Autopilot 
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Figure  59.  Navigational  Updates  Effect:  S^e  As  Figure  58  with  Assumed  Linear 
Variation  Between  Gonsecuiive  Navigational  Updates. 
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Figure  60.  Navigational  Updates  Jffcct:  Same  As  Figure  58  with  <»  =  5. 


113 


environment  when  the  cross  track  error  is  available  only  once  ever\'  thiny  sample 
instances! 

Aniimprovcmcnt  can:be  achieved  if  the  actual-track  deviation  is, assumed:  to  var>^ 
linearly  between  two  consecutive  nivigational  updates.  The  results  arc  presented  in 
Figure  59  where  the  improvement  over  Figure  58  is  evident.  The  response  appears  now 
to  be  bounded  about  the  y  =  0  track.  Funher  improvement  canibe  achieved,  if  more  than 
two  navigational  updates  are  kept  and  a  spline  curve  is  fit  among  them. 

A  final  improvement  is  possible  if  the  value  of  ^  is  increased.  Such  an  increase 
was  found  to  be  advisable  for  noise  suppression  as  well.  Results  for  =  5  arc  presersted 
in  Figure  60  for  a  navigational  update  factor  of  30.  Unlike  the  case  =  0.5  of  Figure 
58,  the  rwponse  is  now  stable.  When  the  linear  extrapolation  technique  of  the  previous 
paragraph  is  combined  with  the  above  increase  in  ^ ,  the  response  is  faster  and  less 
oscillatory,  as  depicted  in  Figure  61.  Introduction  of  the  software  steering  gear  lag, 
=  0.5  seconds,  docs  not  alter  significantly  the  response,  as  shown  in  Figure  62. 
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CONCLUSIONS.AND  RECOMMEDATIONS 

A.  CONCLUSIONS 

A  methodology  for  designing  sliding  mode  autopilots  for  vehicle  maneuvering^and 
track  following  control  has  been  presented.  The  methods  are  suitable  for  a  wide  variety 
of  relatedlcontrol  problems.  Also,  a=  technique  has  been  presented  to  drive  the  vehicle 
onto  the  next  track,  with  no-overshoot  and  minimal  nidder  use,  and  which  can  be  used 
with  any  of  the  control  methods  presented.  Finally,  noise  in  the  measurable  parameters 
Was  evaluated  and.guidelines  for  suppressing  the  effects  of  this  noise  were^presented.  In 
the  present  case  oLthe  AUV  track  keeping,  the  principal  conclusions  of  this  work  can  be 
summarized  in -theifollowing  paragraphs  and  in  FigurtJS-63  and  64; 

As  seen  in  Figure  63,  it  is  shown  that  the  cross  track  error  control  provides  better 
track  keeping  characteristics  4han  heading  (Liiie  of  Sight)  control.  The  premise  of  this 
fesearch  was  the  necessity  for  accurate  track  keeping  of  autonomous  underwater  vehicles 
for  autonomous  navigation  of  a  vehicle  through  confined  spaces,  and  in  the  presence  of 
obstacles  and  cross  current  environments.  Thus,  it  is  paramount  that  AUV’s  have  the 
ability  to  follow  a  track,  with  minimal  cross  track  error,  using  the  control  methods 
developed  in  this  research.  The  Liiie  of  Sight  scheme  is  very  efficient  and  provides 
smooth  turning  characteristics  during  rapid  maneuvering  and  course  changing.  For 
transits  along  straight  line  tracks,  however,  the  stability  of  the  scheme  is  not  guaranteed 
for  every  way  point,  target  distance  combination.  This  is  demonstrated  in  Figure  63 


116 


where  the  initial  heading  is  30° .  Curve  1  coirespohds  to  the  response  of  the  cross  track 
error  design.  The  rest  of  the  curves  correspond  to  stable  responses  of  Line  of  Sight 
designs^  Curve  2  corresponds  to  five  way  points  and  target  distance  d  =  2  vehicle 
lengths,  Curve  3  to  three  way  points  and  d  =  0.1  vehicle  lengths  and  Curve  4  to  two  way 
points  and  d  =  2  vehicle  lengths.  As  can  be  seen,  Curve  1  is:superior  to  all. 

Analytical  evaluation  of  the  stability,  criterion,  in  the  presence  of  a  constant 
disturbance,  was  achieved.  The  cross  track  error  controller  developed  atSteady  state  track 
error  for  this  case, 

For  the  integral  control  method,  it  was; shown  Aat  for  asnonlinear  gain, greater 

than  or  equal  to  :the  theoretical  critical  gain,  there  was  zero  steady  state  error. 

However,  when  the  nonlinear  gain  was  less  . than  the^heoretical  critical  gain,  there  was 
a  finite,  but  stable*  steady-state  error.  When  the  integral  cohtfol  method  was  modified, 
a  zero  steady  state  error  .was  seen  for  a  nonlinear  gain  greater  than  or  equal  to  the 
theoretical  critical' gain,  asTOr  the  integral  control  method.  However,  when  the  nonlinear 
gain  is  less  than  the  theoretical  critical  gain,  a  zero  steady  state  error  results  vice  the 
finite,  stable  steady  state  error,  as  in  the  integral  control  method.  Due  to  the  general 
oscillatory  response  of  the  integraUcontrol  rnethod,  it  is  best  to  keep  the  integrator  pole 
closer  to  the  origin.  When  utilizing  the  integral  control  method,  it  would:  be  best 
eihployed  by  switching  the  integrator  off,  if  the  oscillatory  response  is  too  much  during 
fiansients,  and  by  switching  the  integrator  on  for  long,  straight  transit  tracks. 
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In  the  disturb^ce  estimation-  and  compensarion  method,  a  zero  steady  state  error 
resulted  when  the  nonlinear  gain  was  greater  than  or  equalHo  the  theoretical  critical  gain. 
An  unstable:response  resulted  for  this  control  method  when  the  nonlinear  gain; was  less 
than  the  th^retic^  critical  gain.  For  the  modified  disturbance  estimation  and 
compensation  method,  a  zero  steady  state  error  again  resulted  for  the  nonlinear  gain, 
greater  than  or  equal; to  the  theoretical  critical  gain;  Now  for  the  nonlinear  gain  less  than 
the  theoretical  critical  gain,  a  stable  nonzero  steady  state  error  resulted.  In  general,  the 
response  is  not  oscillatory  for  the  disturbance  estimation  and  cornpensation  method,  but 
this  method  only  controls  the  cross  track  error,  due  to  a  constantidisturbance,  when  the 
constant  disturbance  is  a  current.  On  the  other  hand,  the  integral  control  method  can 
control  the  cross  track  error,  due  to  any  constmt  disturbance,  not  only  a  current.  These 
results  are  seen  in  Figure  64.  The  vehicle  was  subjected  to  a  sway  force  disturbance 
equivalent  to  a  1  ft/sec  cuitent  and  a  yaw  moment  disturbance  equivalent  to  a  2  ft/sec 
current,  thus-  the  constant  disturbances  do  not  correspond  to  any  physically  realizable 
currents.  The  integral  control  method  (Curve  1)  brings  the  vehiclesonto  the  desired  track, 
whereas,  the  disturbance  estimation  and  compensation  method  (Curve  2)  and  the  plain 
cross  track  error  designs  (Curve  3)  both  experience  nonzero  steady  state  errors.  Of 
course,  if  the  disturbance  observer  is  modified  to  take  into  account  a  general  sway  force 
and  yaw  moment,  then  the  response  would  experience  zero  steady  state  error  as  the 
integral  control  method  does. 

The  leading  track  control  technique  was  seen  to  improve  thenurning  characteristics 
of  the  vehicle,  so  as  not  to  overshoot  the  next  track  and  to  use  minimal  rudder.  The 
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distance  from  the  way  point  to  accomplish  this  was  determined  automatically  by  the 
technique.  This  technique  can  be  used  with  all  the  confrol  methods  developed:  in  this 
research. 

The  cross  track  error  controller  proved  to  be  very  robust  and  was  able  to  handle  a 
wide  range  of  parameter  variations  without  loss  of  stability. 

The  effects  of  sensor  noise  and  sensor  drift  Were  numerically  evaluated.  With 
appropriate  modifications  in  the  control  law,  ;it  was  shown  that  sensor  noise  and  sensor 
drift  could;  be  minimized.  Finally,  it  was  demonstrated  that  positional  updates  are  ver>' 
important  for  accurate  track  keeping,  but  they  can  occur  at  a.  slower  rate  than  the  rest  of 
therautopilot  updates. 

B.  RECOMMENDATIONS 

Some  recommendations  for  further  research  are  as  follows: 

•  Experimental  verificatiorimsing  the  fulfscale  NFS  AUV  II  after  its  hydrodynamic 
coefficients  have  been  reliably  established. 

•  Incorporation  of  Kalman  filter  designs  to  further  improve  the  response  and  to 
reduce  the  effects  of  sensor  noise  and  randomidisturbances. 

•  Simulation  of  an  Inertial  Navigation  System  required  for  positional  updates. 
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APPENDIX  A 


C  ADDED  CURRENT  AS  A  DISTURBANCE  IN  THE  CONTROL  LAW 
C  ADDED  CURRENT  OBSERVER 

C  ADDED  XI  TO  MODIFY  THE  DISTURBANCE  AND  ESTIMATION  COMPENSATION 
C  METHOD 
C 

REAL  HASS,LATYAW,NORPIT 

REAL  MM(6,6) ,G4(4) ,GK4(4) ,BR(9) ,HH(9) 

REAL  B(6,6)vBB(6,6) 

REAL  A(12,12),  AA(12,12),TNDX(100) 

REAL  XPP  ,XQQ-  ,XRR  iXPR 
REAL  XUDOT  ,XWQ  ,XVP  ,XVR 
■  REAL  XQDS  ,XQDB  ,XRDR  ,XVV 
REAL  XWW  ,XVpR  ,XWDS  ,XWDB 
REAL  XDSDS,XOBDB  ,XDRDR  ,XQDSN 
REAL  XWDSN  ,XDSDSN 

REAL  TIME, S,EITA,UBAR,UHAf,COMZ, BAR, SIM, DE, SAT, VHAT,ZZ0BS,Z20BSD0T,SIM1 
REAL  SSI ,  SS2 , Ub ,  XD  /YD ,  TD ,  TNWP ,  XA ,  YA ,  HD ,  HDMDEG ,  DAWAY ,  SATSGNl ,  SATSGN2 
REAL  NAVUPDATE , TNAV , TARGET , rF,GG,HHH,LLL,HDE, HDM , VCUR , UCUR , UCO , VCO , WCO 
REAL  UCOOBS,VCOOBS,VCOHAT,UCHAT 
INTEGER  DV 
C 

C  LATERAL  HYDRODYNAMIC  COEFFICIENTS 

C 

REAL  YPDOT  ,YRDOT,YPQ  ,YQR 
REAL  YVbOT  , YP'  , YR  , YVQ 
REAL  YWP  ,YWr  ,YV  ,YVW 
REAL  YDR  ,CDY 
C 

C  NORMAL  HYDRODYNAMIC  -COEFFICIENTS 

C 

REAL  ZQDOT  ,ZPP,ZPR  .,ZRR 
REAL  ZWDOT  ,ZQ  ,ZVP  ,ZVR 
REAL  ZW.  ,ZW  ,ZDS  ,ZDB 
REAL  ZON  ,ZWN  ,ZDSN-  ,CDZ 
REAL  ZHADOT,ZHAT 
C 

C  ROLL  HYDRODYNAMIC  COEFFICIENTS 

C 

REAL  KPDOT  , KRDOT  ,KPQ  ,  KQR 
REAL  KVbOT  ,  KP  ,KR  ,KVQ 
REAL  KWP  ,  KWR  , KV  , KVW 

REAL  KPN  ,  KbB 
C 

C  PITCH  hydrodynamic  COEFFICIENTS 

C 

REAL  MQDOT  ,MPP  ,HPR,MRR 
REAL  MWbOT  ,  MQ  ,MVP  ,MVR 
REAL  MW  ,  MW  ,HbS  ,MDB 
REAL  MQN  ,  KWN  ,HDSN 
REAL  QHADOT,QHAT,THADOT,THAT 
C 

C  YAW  HYDRODYNAMIC  COEFFICIENTS 

C 

REAL  NPDOT,NRbOT,NPQ  ,NQR 
REAL  NVDOT  ,  NP  ,’NR  ,NVQ 
REAL  NWP  ,  NWR  ,NV  ,NVW 
REAL  NDR 
C 

C  MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

C 
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REAL  WEIGHT  ,  BOY  ^VOL  ,XG 

REAL  YG  ,  ZG  ,XB  ,ZB 

REAL  IX  ,  lY  ,IZ  ,IXZ 

REAL  lYZ  ,  IXY  ,YB 

REAL  L  ,  RHO  ,G  ,NU 

REAL  AO  ,KPROP  ,NPROP  ,  XlTEST 

REAL  DEGRUD  ,DEGSTN 

COMMON  /BLOCKl/  F(12),  FP{6),  XMMINV{5y6),  UCF 

INTEGER  N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

REAL  VECVl (9 ) , VECV2 ( 9 ) , X ( 12 ) , VECHl (9 ) , VECH2 ( 9 ) , XI ( 9 ) 

RUDDER  COEFFICIENTS 

PARAMETER  (  DSMAX-  -0.175) 

LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER{XPP  -  7.E-3  ,XQQ--1.5E-2  ,XRR  -  4.E-3  ,XPR-7.5E-4, 

6  XUDOT— 7.6E-3  ,XWQ  -  -2.E-r  ,XVP  -»  -3.E-3  ,XVR  -  2.E-2, 

&  XQDS-2.5E-2  ,XQDB— 2.6E-3:  ,XRDR-  -l.E-3  ,XW  -5.3E-2, 

&  XWW  -1.7E^1  ,XVDR-1.7E-3  ,XWDSi;4 . 6E-2  ,XWDB-  l.E-2, 

&  XDSDS-  -l.E-2  ,XDBDB-  -8.E-3  .XDRDR-  -l.E-2  ,XQDSN-  2.E-3, 

&  XWDSN-3.5E^3  ,XDSpSN-  -1.6E-3  ) 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(YPD0T-1.2E-4  ,YRDOf!il.2E-3  .^YPQ  -  4iE-3  ,YQR  i-6.5E-3, 

&  Y.VDOT—5.5E-2  ,YP  -  3.E-3  ,YR  -3.E-2  ,YVQ  -2.4E-2, 

6  YWP  -2.3E-1  ,YWR— 1.9E-2-  ,YV  -  -^l.E-l  ,YVW  -6i8E-2, 

&  YDR  -2.7E-.2  ,CDY  -3.5E-1) 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(ZQDOT— 6.8E-3  ,ZPPil.3E-4  ^ZPR-6.7E-3  ,ZRR- — 7.4Er3, 
S  ZWDOT— 2.4E-1  ,ZQ  -^1.4E-1  ,ZVP  -i-4.8E-2  ,ZVR  -4.-5E-2, 

S  ZW  -  -3iE^i  ,ZVV— 6.8E-2  ,ZDS  — 7.3E-2  ,ZDB  -42.6E-2, 

S  ZQN  — 2.9E-3  ,ZWN  — 5.1E-3  ,ZDSN-  -l.E-2  ,CDZ  -  1.0) 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER (KPDOT-  -l.E-3  , KRDOT— 3 . 4E-5  ,KPQ  — 6.9E-5  ,KQR  -1.7E-2, 
S  KVDOT-1.3E-4  ,  KP— l.lE-2  ,KR  --8.4E-4  ,KVQ— 5.1E-3, 

5  KWP— 1.3E-4  ,  KWR  -1.4E-2  ,KV-3.1E-3  ,KVW  --1.9E-1, 

6  KPN  — 5.7E-4  ,  KDB  -  0.0  ) 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(MQDOT— 1.7E-2  ,MPP  -5.3E-5  ,MPR  -  5.E-3  ,MRR  — 2.9E-3, 

5  MWbOT— 6i8E-3  ,  MQ— 6.8E-2  ,MVP  -l,2E-3  ,MVR-1.7E-2, 

6  MW  •  l.E-1  ,  MW  — 2.6E-2  ,MDS  — 4.1E-2  ,MDB  -6.9E-3, 

&  MQN  — 1.6E-3  ,  MWN  — 2.9E-3  ,MDSN  — 5.2E-3) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PA^^ETER(NPDOT— 3.4E-5  ,NRbOT— 3 . 4E-3  ,NPQ  — 2.1E-2  ,NQR  -2.7E-3> 

&  NVDOT-1.2E-3  ,  NP  — 8.4E-4  ,NR  — 1.6E-2  ,NVQ  -  -l.E-2, 

i  NWP  — 1.7E-2  ,  NWR  -7.4E-3  ,NV  S-7.4E-3  ,NVW  — 2.7E-2, 

&  NDR  — 1.3E-2) 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 
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PARAMETER(  WEIGHT  -12000.  BOY  -12000.  ,V0L  -200.  ,XG  -  0. 


&  YG  -  0.0 

,  ZG  -  0.20 

,XB  -  0. 

, ZB  —  0.0  , 

&  IX  -  1500. 

,  lY  -  10000. 

,IZ  -  10000. 

,IXZ  -  -10.  , 

&  lYZ  -  -10. 

,  IXY  -  -10. 

,YB  -  0.0  , 

t  L  -  17.4 

,  RHO  -  1.94 

,G  -  32.2 

,NU  -  8.47E-4 

t  AO  -  2.0 

,KPROP  -  U. 

,NPROP  -  0.  , 

XlTEST-  0.1  . 

i  DEGRUD-  0.0  ,DEGSTN-  ’O.O) 

INPUT  INITIAL  CONDITIONS  HERE  IP  REQUIRED 

OPEN( 20 , FILE- ' MODELl . DAT STATUS- ' NEW' ) 

NUMPTS-0.0 

DV-I.O 

********** ***********(55tain  INITIAL  INFORMATION***************** 

OPEN  (30, FILE- 'INITIAL. DAT '.STATUS- 'OLD' ) 

READ  (30,  *  )  UO.RPM 

READ  (30,*)  UD,NAVU?DATE,SIM1,DELT 

READ  (30,*)  XD2,YD2,COMZ 

.  .  i  READ  IN  STEERING  AND  SLIDING  SURFACE  GAINS , INITIAL  CURRENTS, 
.  ...  AND  SATURATION  DESIRED 

0PEN(21,F:LE-'SMCINT.DAT' /STATUS- 'OLD' ) 

READ (21,*)  GG1,GG2,GG3,GG4,GG5 
READ(  21  ,.* )  SPl ,  SP2 ,  SP3 ,  SP4 ,  SPS 
READ (-21,*)  UCO,VCO,WCO 
READ  (-2 1 ,  * )  AKN ,  S  S  PHI 
READ(;21,*)  IPT3,XI 

UCOOBS-0.0 
VCOOBS-0 . 0 
VO  -  0.0 
WO  -  0.0 
PO  -  0.0 
00  -  0.0 
RO  -  0.0 
PHIO  -  0.0 
thetaO  -  0.0 
PSIO  -  0.0 
XPOSO-0.0 
YPOSO-O.O 
ZPOSO-O.O 
XDl-O.O 
YDl-O.O 
DB-  0.0 
DS  -  0.0 
DR  -  0.0 
LATYAW  -  0.0. 

NORPIT  -  0.0 
RE  -  U0*L/NU 
TNAV-0 
XA-XPOSO 
YA-YPOSO 

TIME-0.0 

TIMEO-O.O 
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u  -  uo 

V  ■  VO 

w  -  wo 

P  -  PO 
Q  -  QO 
R  -  RO 

XPOS  -  XPOSO 

ypos  -  ypbso 

ZPOS  -  ZPOSO 
PSI  -  PHIO 
THETA  -  THETAO 
PHI  -  PHIO 
QHADOT-0 . 0 
THADOT-0 . 0 
ZHADOT-0 . 0 
QHAT-0.0 
THAT- 0.0 
;ZHAT-0 .0 
VHAT-O.b 
ZOBSDOT-0 .0 
ZZOBS  i  0.0 

i..  DEFINE  THE  XENGTH.  X  AND  HEIGHT  HH  TERMS  FOR  THE  DRAG  INTEGRATION 


XKD 

m 

-105.9/12. 

Xl(2) 

-99.3/12. 

XI  (3 ) 

m 

^87.3/12. 

XI  (.4) 

m 

-66.3/12. 

XI  (-S.) 

m 

72.7/12. 

Xl{6) 

m 

83.2/12. 

XI  (7) 

m 

91.2/12. 

Xl(8) 

m 

99.2/12. 

XI  (-9) 

m 

103.2/12., 

HH(1) 

m 

0.00/12. 

HH(2) 

8.24/12. 

HH(3) 

m 

19.76/12. 

HH(4) 

m 

29.36/12. 

HH(5) 

m 

31.85/12. 

HH(6) 

m 

27.84/12. 

HH{:7) 

m 

21.44/12. 

HH(8) 

m 

12.00/12. 

HH(9) 

m 

0.00/12. 

BR(1) 

m 

0.00/12.0 

BR{2) 

8.24/12.0 

BR(3) 

m 

19.76/12.0 

BR(4) 

m 

29.36/12.0 

BR(5) 

m 

31.85/12.0 

BR(6) 

m 

27.84/12.0 

BR(7) 

m 

21.44/12.0 

BR(8) 

m 

12.00/i2.0 

ieR(9) 

m 

0.00/12.0 

MASS  -  WEIGHT/G 

N  -  6 

DO  15 

j 

-  1,N 

DO 

1C 

K  -  1,N 

XMMINV(J,K)  -  0.0 
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KM(J,K)  -  0.0 

10  CONTINUE 
15  CONTINUE 

C 

DEFINE  HASS  MATRIX 

MH(1,1)  -  MASS  -( (RHO/2)*(L**3)*XUDOT) 

MH{1,5)  -  MASSno 
MM (1,6)  -  -KASS*yG 

MM(2,2)  -  MASS  -( { RHO/2 ) *{ L**3 ) *YVDOT) 

MM(2,4)  -  -MASS*ZG  -( (RHO/2  )*  {I.**4  )*YPDOT) 
MM;(2,6)  -  MASS*XG  -  (( RHO/2 )*( L**4  ) *YRDOT) 

MM(3,3)  -  MASS  -  ( (RHO/2 )*(L»*3)*ZWDOT) 

HH(3,4)  -  MASS*YG 

MM(3,5)  -  -MAUS*XG  -(( RHO/2 )* (L**4 ) *ZQDOT) 

MH(4,2)  -  -MASS*2G  -  (^(  RHO/2  )*(  L**4  )  *KVDOT) 
MM(4,3)  -  MASS*VG 

MM (4, 4)  -  IX  -  (( RHO/2) *(L**5)*KPD0T) 

HH(4,5)  -  -ixy 

MM(4,6)  -  -IXZ  -n RHO/2) *(L^*5)*KRDOT) 

MM(5,1)  -MASS*ZG 

MM(=5,3)  -  -MASS*XG  -(  (RHO/2)*(L**4)*HWdOT) 
MM(5,4)  -  -m 

mm(5,5)  -  ly  -((rho/2)*(l**5)*mqdot) 

MM(5,6)  -  -lyz 

HM(6,1)  -  -MASS*YG 

MM{6,2)  -  MASSING  -( (RHO/2 )*(L**4)*NVDOT) 
MM(6,4)  -  -IXZ  -  { (RHO/2 )»(L**5)*NPD6t) 

MM(6,5)  -lYZ 

MM(6,6)  -  IZ  -  (:(RH0/2)*(L**5)*NRD0T) 

OBSERVER  POLES 

51- -1.0 

52—  1 . 1 
.'3—1.2 

OBSERVER  A  MATRIX  CONSTANTS  AND  B  MATRIX  CONST/iNTC 

All— 0.045.30 
A12— 0.35119 
A21— 0.002795 
A22— 0.09568 
B1  -  0.011432 
B2  —0.004273 

*****ROUTINE  FOR  INVERTING  THE  MM  MATRIX***** 

DO  12  I-1,N 
DO  11  J-1,N 

XHMiNV(I,J)-0.0 

11  CONTINUE 
XMMINV(I,I)-1 

12  CONTINUE 

CAUL  INVTA(MM,N,INDX,D) 
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DO  13  Jil,N 

CALL  INVTB{MM,N,INDX,XMMINV(1, J) ) 

3  CONTINUE 

************  INPUTS  ************* 

RUDDER  AND  DIVE  PLANE  COMMANDS 

SIM-SIMI/DELT 
TIME-0.0 
DS-  0.0 
DR-  0.0 
DB-  0.0 
EITA-4.0 
BAR-. 4 

YINTGR-O.O 
SSPHM— SSPHI 

. .  .  DETERMINE  THE  ANGLE  ALPHA ;XPOS  AND  YPOS  ARE  GLOBAL  COORDINATES 

XP.OSl-XDl 

YPOSl-YDl 

XPbS2-XD2 

YPOS2-YD2 

call  ANGLE{ XPOSl , YPOSl , XPbS2 , YP0S2 , ALPH ) 

. ..  DETERMINE  THE  LENGTH  OF  INITIAL  PATH 

XT-SQRT(  (XPOS2t-XPOS1)**2  +  (  YP0S2-YP0Sr)  **2 ) 

XT-XT*L 

*******************SIM0LATI0N  BEGINS  **************** 

DO  100  I-1,SIM 
PROPULSION  MODEL 
SIGNU  -  liO 

IF  (U.LT.0.0)  SIGNU  -  -1.0 
IF  (ABS(U).LT.XltEST)  U  -  XlTEST 
SIGNN  -  1^0  _ 

IF  (RPM.LT.O.O)  SIGNN  -  -1.0 
ETA  -  0.012*RPM/U 
RE  -  U*L/NU 

CDO  -  .00385  +  (1.296E-17)*(RE  -  1.2E7)**2 

Ct  -  ABS(0.008*L**2*ETA*ABS(EtA)/(A0) ) 

Cti  -ABS(  0.008*L**2/(A0) ) 

EPS  -  -1 . 0+SIGNN/SIGNU* ( SQRT( CT+1 . 0 ) -1 .0 )/( SQRT( CTl+1 . 0 ) -1 . 0 ) 
XPROP  -  CpO*(ETVABS(ETA)  -  1.0) 

.  ..  CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
DO  500  K-i,9 

UCF-{V+X(K)*R)**2+{W-X(K)*Q)**2 

UCF-SQRT(UCF) 

IF  (UCF.LT.I.E-6)  go  to  601 

CFLOW  -CDY*HH(K)*(V+X(K)*R)**2+CDZ*BR(K)*(W-X(K)*Q)**2 
VECHl ( K ) -CFLOW* ( V+X ( K ) *R ) /UCF 

VECH2(K)-CFLOW*{V+X(K)*R)*X(K)/UCF  _  . 
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VECVl ( K ) -CFLOW* ( H-X( K ) *Q )/UCF 
VECV2 ( K ) -CFLOW* ( W-X ( K ) *Q ) *X ( K )/UCF 
500  CONTINUE 

CALL  TRAP (9, VECVl, X, HEAVE) 

CALL  TRAP ( 9, VECV2,X, PITCH) 

CALL  TRAP(9,VECH1,X,SWAY  ) 

CALL  TRAP(9,VECH2,X,YAW  ) 

SWAY— 0 . 5*RHO*SWAy 
YAW— 0.5*RHO*YAW 
HEAVE— 0 . 5*RH0*HEAVE 
PITCH-+0.5*RHO*PITCH 
GO  TO  602 

601  HEAVE-0. O 
PITCH-0.0 
SWAY  -0.0 
YAW  -0.0 

602  CONTINUE 

FORCE  EQUATIONS 


LONGITUDINAL  FORCE 

FPd')  -  HASS*V*R  -  MASS*W*Q  +  MASS*XG*Q**2  +  MASS*XG*R**2- 
S  MASS*YG*P*Q  -  nASS*ZG*P*R  +  ( RHO/2 ) *L**4* {XPP*P**2  + 

5  XQQ*Q**2  +  XRR*R**2  +  XPR*P*R)  +(RHO/2)*L**3*(XWQ*W*Q  + 

6  XVP*V*P+XVR*V*R+.0*Q*(XQDS*DS+XQDB*DB)+XRDR*U*R*DR)+ 

4  (RHO/2)*L**2*(XVV*V**r  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W* 

4  (XWDS*DS+XWDB*DB)+.U**2*(XDSDS*DS**2+XDBDB*DB**2+ 

4  XDRDR*DR**2) )-(WEIGHT  -BOY) *SIN( THETA)  +{ RHO/2 ) *L**3* 

4  XQDSN*U*Q*DS*EPS+( RHO/2 )*L**2*(XWDSN*U*W*DS+XDSDSN*U**2* 

4  DS**2)*EPS  +(RHO/2)*L**2*U**2*XPROP 

LATERAL  FORCE 

FP(2)  -  -MASS*U*R  -  MASS*XG*P*Q  +  MASS*YG*R**2  -  MASS*ZG*Q*R  + 

4  ( RHO/2 )*L**4*(YPQ*P*Q+  YQR*Q*R)+( RHO/2 ) *L**3* ( YP*U*P  + 

4  YR*U*R  +  YVQ*V*Q  +  YWP*W*P  +  YWR*W*R)  +  (RHO/2) *L**2* 

4  (YV*U*V  +  YVW*V*W  +YDR*U**2*DR)  +SWAY  +( WEIGHT-BOY ) * 

4  COS(THETA)*SIN(PHI)+MASS*W*P+MASS*yG*P**2 

NORMAL  FORCE 

FP(3)  -  MASS*U*Q  -  MASS*V*P  -  MASS*XG*P*R  -  MASS*YG*Q*R  + 

4  MASS*ZG*P**2  +  MASS*ZG*Q**2  +  { RHO/2 ) *L**4* ( ZPP*P**2  + 

4  ZPR*P*R  +  ZRR*R**2)  +  ( RHO/2 ) *L**3* ( ZQ*U*Q  +  ZVP*V*P  + 

4  2VR*V*R)  +.( RHO/2 )*L**2*(ZW*U*W  +  ZVV*V**2  +  U**2*(ZDS* 

4  DS+ZDB*DB ) ) +HEAVE+(WEIGHT-BOY) *COS( THETA) *COS( PHI )+ 

4  (RHO/2) *L**3*ZQN*U*Q*EPS  +( RHO/2 ) *L**2* ( ZWN*U*W  +ZDSN* 

4  U**2*DS)*EPS 

ROLL  FORCE 

FP(4)  -  -IZ*Q*R  +IY*Q*R  -IXY*P*R  +IYZ*Q**2  -IYZ*R**2  +IXZ*P*Q  + 
4  MASS*YG*U*Q  -MASS*YG*V*P  -MASS*ZG*W*P+( RHO/2 ) *L**5* ( KPQ* 

4  P*Q  +  KQR*Q*R)  +( RHO/2 ) *L**4 *( KP*U*P  +KR*U*R  +  KVQ*V*Q  + 

4  KWP*W*P  +  KWR*W*R)  +(RHO/2)*L**3*(kV*U*V  +  KVW*V*W)  + 

4  (YG*WEIGHt  -  YB*BOY ) *COS( THETA )*COS( PHI)  -  (ZG*WEIGHT  - 

4  ZB*BOY)*COS(THETA)*SIN(PHI)  +  ( RHO/2) *L**4*KPN*U*P*EPS+ 

4  (RHO/2)*L**3*U**2*KPROP  +MASS*ZG*U*R 
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PITCH  FORCE 

rP(5)  -  -IX*P*R  +T2*P*R  +IXY*Q*R  -iyZ*P*Q  -1XZ*P**2  +IXZ*R**2  - 
tt  «ASS*XG*U*Q  +  nASS*XG*V*P  +  MASS*ZG*V*R  -  MASS*ZG*W*Q  + 

i  (RH0/2)*L**5*(MPP*P**2  +MPR*P*R  +MRR*R**2)+(RH0/2)*L**4* 

i  (HQ*U*Q  +  MVP*V*P  +  MVR*V*R)  +  ( RHO/2 ) *L**3*  ( MW*U*W  + 

t  :MW*V**2+U**2*(MDS*DS+MDB*DB)  )+  PITCH  -(XG*WEIGHT^ 

«  XB*BOY)*COS(THETA)*COS(PHI)  +  (RHO/'2)*I,**4*MQN*U*Q*EPS  + 

*  (RHO/2 )  *L**3*  (MWN*U*M4-MDSN*U**2*bS )  *EPS- 

«  ;(ZG*WEIGHT-ZB*BOy)*SIN(THETA) 

YAW  FORCE 

FP(6)  -  -IY*P*Q  +IX*P*Q  +IXY<'P**2  -IXY*Q**2  +IYZ*P*R  -IXZ*Q*R  - 
&  .MASS*XG*U*R  +  MASS*XG*W*P  -  MASS*YG*V*R  +  MASS*YG*W*Q  + 

&  (RHO/2)*L**5*(NPQ*P*Q  +  NQR*Q*R)  +{ RHO/2 ) *L**4* ( NP*U*P+ 

«  NR*U*R  +  NVQ*V*Q  +NMP*M*P  +  NWR*W*R)  +( RHO/2 ) *I,**3* ( MV* 

&  U*V  +  NVW*V*W  +  NDR*0**2*DR)  +  YAM  +  {XG*WEIGHT  - 

5  XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA) 

6  +( RHO/2 )*L**3*U**2*NPR0P-YB*B0Y*SIM( THETA) 

NOW  COMPUTE  THE  r(i-6)  FUNCTIONS 

DO  600  J  m  1,6 

F(J)  .  0.0 
DO  600  K  -  1,6 

F(J)  i  XMMINV(J,K)*FP(K)  +  F(J) 

CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 
INERTIAL  POSITION  RATES  F(7-9) 

F(7)  -  UCO  +  U*COS(PSr)*COS{THETA)  +  V* ( COS ( PSI ) *siN( THETA) * 
i  SIN(PHI)  -  SIN(PSI)*COS(PHr)  )  +  W* ( COS(PSI ) *SIN( THETA) * 

&  COS(PHI)  +  SIN(PSI)*SIN(PHI')) 

F(8)  -  VCO  +  U*SIN(PSI)*COS(fHETA)  +  V* ( SIN( PSI ) *SIN{ THETA) * 

&  SIN(PHI)  +  COS(PSI)*C6S(PHr))  +  W*(SIN(PSI)*SIN(THETA)* 

6  COS (PHI)  -  COS(PSI)*SIN(PHI) ) 

F(9)  -  HCO  -  U*SIN(THETA)  +V*COS(THETA) *SIN(PHI )  +H*COS( THETA) * 
6  COS (PH I) 

EULER  ANGLE  RATES  r( 10-12) 

F(10)  i  P  +  Q*SIN(PHI)*TAN(THETA)  +  R*COS( PHI ) *TAN(THETA) 

F(ll)  -  Q*COS(PHlj  -  R*SIN(PHI) 

F(12)  -  Q*SIN(PHI)/COS(THETA)  +  R*COS ( PHI )/COS ( THETA) 


UDOT  -  F(l) 
VDOT  -  F(2) 
MOOT  -  F(3) 
PbOT  -  F(4) 
QDOT  -  F(5) 
ROOT  -  F(6) 
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XDOT  -  r ( 7 ) 

YDOT  -  r ( 8 ) 

ZDOT  -  r(9) 

PHIDOT  -  r(10) 

THETAD  -  F(ll) 

PSIDOT  -  r(12) 

C 

C  *******  CREATE  OUTPUT  DATA  PILE  ************************ 

C 

IF  (I  .EQ.  DV  )  THEN 
TIMER-FLOAT(T)/2. 

WRITE  (20,*)  I 

WRITE  (20,743)  DR/. 01745 

WRITE  (20,744)  XPOS/L,YPOS/L,XD2,yD2 

WRITE  (20,746)  (PSI-ALPH)/. 01745, YLCASE/L 

WRITE  (20,746)  VCOOBS , UCOOBS 

743  FORMAT  s( El  1.3) 

744  FORMAT  :(  4 El 2. 4) 

746  FORMAT  (2E12. 4) 

C 

NUMPTS-NUMPTS  +  1 
DV-DV+1 ^ 0/DELT 
ENDIF 
C 

C***** 

C  FIRST  ORDER  INTEGRATION 

C 


u  - 

U  +  DELT*UDOT 

c 

U 

m 

SURGE  RATE 

V  - 

V  ■(•  ■DELT*VDOT 

c 

V 

m 

SWAY  RATE 

W  i 

W  +  :DELT*WD0T 

c 

w 

m 

HEAVE  RATE 

P  i 

P  +  bELT*PDOT 

c  • 

p 

m 

ROLL  RATE 

Q  p 

Q  +  DELT*QDOT 

c 

Q 

m 

PITCH  RATE 

R  ? 

R  +  •DELT*RDOT 

c 

R 

m 

YAW  RATE 

XPOS 

-  XPOS  +  DELT*XDOT 

c 

X 

m 

|URGE 

YPOS 

-  YPOS  +  DELT*YDOT 

c 

Y 

m 

SWAY 

ZPOS 

-  ZPOS  +  DELT*ZDOT 

c 

Z 

m 

HEAVE 

PHI 

-  PHI  +  DELT*PHIDOT 

c 

PHI 

-  ROLL 

THETA  -  THETA  +  DELt*THETAD 
C  THETA  -  PITCH 

PSI  -  PSI  +  DELT*PSIDOT 

C  PSI  -  YAW 

C 

YINTGR-YINTGR  +  DELT*YLCASE 
C 

C  *********SI,IDING  MODE  DEPTH  CONTROL************* 

C 

CALL  OBSER ( QHADOT , THADOT , 2HAD0T , QHAT , THAT , ZHAT ,DELT,ZPOS,DS,U0) 
C 

S-QHAT  +  0.52*THAT  -  0 . 0112* ( ZHAT-COMZ*L) 

IF(ABS(S)  .LT.  BAR)  SAT-(S/BAR) 
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IF:(S  .LE.  -BAR)  SAT— 1.0 

IF,{S  .GE.  BAR)  SAT-1.0 

UHAT— 5.1429*QHAT  +  1.0714*THAT 

UBAR-EITA*SAT 

DE-UHAT+UBAR 

IF  (DE  .GE.  0.4)  DS-0.4 

IE  (DE  .LE.  -0.4)  DS— 0.4 

IF(  (DE  .LT.  0.4).  .AND.  (DE  .GT.-  -0.4))  DS-DE 
DB— DS*1.0 

*********slIDING  MODE  STEERING  CONTROL************ 

*******planner** ******* 


...  .  DETERMINE  REQUIRED  POSITION 

CALL  HEAD( L, XPOS , YPOS .XPOSl , YPOSl , ALPH , XLCASE, YLCASE ) 

.  DETERMINE  IF  XLCASE  I.S  WITHIN  L/2  DISTANCE  OF  D 

DAWAY-ABS ( XLCASE-XT ) 

IF  (  DAWAY  .LE.  2.0*L  )  THEN 

WRITE(*;,*)  'CURRENT  POSITION  IS  '  ,XPOS/L,  YPOS/L ,  ZPOS/L 

WRITE (*,*)  'SIMULATION  TIME  IS  '  ,  I 

XD1-XD2 

YD1-YD2 

READ  (30,*)  XD2,YD2,COMZ 

IF  ((XD2  .EQ.  0.0)  ,AND.(YD2  .EQ.  0.0)  .AND. 

$  (C0;iz  .EQ.  0.0)  )  GO  TO  3 

.  ...  CONVERTS  LOCAL  COORDINATES  INTO  GLOBAL  COORDINATES 

XPOSl-XDl 

YPOSl-VDl 

XPOS2-XD2 

YPOS2-yb2 

.  . .  DETERMINE  THE  NEW  ANGLE  ALPHA  FOR  THE  NEW  WAY  POINT 

CALL  ANGLE ( XPOS 1 , YPOSl, XPOS2 , yPOS2 , ALPH ) 
VC-VC06bS*C0S(ALPK)-UC00BS*SIN(ALPH) 

UC-UCOOBS*COS ( ALPH )-VCOOBS*SIN( ALPH) 

...  ,  CALCULATE  THE  LENGTH  OF  THE  NEW  PATH 

XT-SQRT( (YPOS2-YPOSl)**2  +  ( XPOS2-XPOS1 ) * *2 ) 

XT-XT*L 

.  . .  DETERMINE  NEW  XLCASE  AND  YLCASE  FOR  THE  NEW  WAY  POINT 

CALL  HEAD( L , XPOS , YPOS , XPOSl , YPOS 1 , ALPH , XLCASE , YLCASE ) 
Z2-VC+S2*YLCASE 

Z3-UC+S3*XLCASE+U*COS(PSI-ALPH) 

.  . .  RESET  YINTGR  FOR  NEXT  WAY  POINT 

YINTGR-0.0 

ENDIF 

******navigator******* 
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IF  (:(,TIME-TNAV)  .GE. 
XA-XID 


YA-YID 

TNAV-TNAV+NAVUPDATE 

ENDIF 


NAVUPDATE  )  THEN 


****desired  speed*** 

UD  IS  SPECIFIED  AND  HELD  CONSTANT 

********RPM  INPUT  CALCULATION  ********* 

SSl-U-UD 

IF(ABS(SS1)  .LT.  1.0)  SATSGN1-(SS1/1 ) 

IF(SS1  .LE.  -1.0)  SATSGNl— 1.0 
IF(SST  .GE.  1.0)  SATSGNl-1.0 
RPM— 1153.9*SATSGN1  +  83.33*0 
IF  (RPM  .GE.  500.0)  RPM-  500.0 
IF  (RPM  .LE.  -500.0)  RPM— 500.0 

*************************  *CURRENT  OBSERVER*  *  ***************************** 


Z1D0T-S1*21+(S1*(A11+A22)/A21+(A12-A11*A22/A21")*U- 
$  S1*S1/(A21*U)  )*R+(B1*U-B2*(A11»U-Sl;)/A2L)  ‘U^DR 

22DOT-S2*Zl+S2*22+S2*U*SIN(PSI-ALPH)-S2*S2*YLCASEx 
?  (S2*(A11*U-ST)/(A21*U) )*R 

23DOT-S3*23-S3*S3*XLCASE 

21- 21+21DOT*DELT 

22- Z2+22D0T*DELT 

23- 23+23DOT*DELT 
VHAT-21+R*(A11*U-S1)/(A21*0) 

VCOHAT- 2 2 - S 2 * VLCAS E 

UCHAT-2-3tS3*XLCASE 

VCOOBS-VCOHAT 

UC00BS-UCHAT-U*C0S ( PSI-ALPH ) 

*  *  *  *  *  *.*  ***************************************************************** 


*******RUDDER  INPUT  CALCULATION******* 

DANGLE- (PSI-ALPH) 

IF  (DANGLE  .GE.  6.2832)  THEN 
DANGLE-DANGLE-6 . 28 3 2 
ENDIF 

VCl-VCOOBS/U 
IF  (VCl  .GE.  1.0)  THEN 
VCl-1.0 

ELSEIF  (VCI  .LE.  -1.0)  THEN 
VCl  — 1.0 
ENDIF 

SS2-SP1*( DANGLE )+SP2*VHAT+SP3*R+SP4*YLCASE+ 

&  ( (SSPHI*GG1 )/AKN  +  SPl ) *ASIN( VCl ) 

C 

IF(ABS(SS2)  .LT.  SSPHI  )  SATSGN2-( SS2/SSPHI ) 

IF(SS2  .LE.  SSPHM)  SATSGN2— 1 . 0 
IF(SS2  .GE.  SSPHI)  SATSGN2-1.0 
C 

DR-AKN*SATSGN2+(GG1+(XI )*SP1)*(DANGLE)+(GG2+(XI)*SP2) *VHAT 
S  +(GG3+(XI )*SP3) *R+(XI )*SP4*YLCASE+GG5*YINTGR 
C 

IF  (DR  .GE.  0.4)  DR  -  0,4 


131 


noo  ooo  ooo  o  o  on 


IF  (DR  .LE.  -  0.4)  DR  -  -0.4 
C 

TIME-TIME+DELT 

C 

PHIANG  -  PHI/0.0174532925 
THEANG  -  THETA/0. 0174532925 
PSIANG  -  PSI/0. 0174532925 
C 

ALPANG-ALPH/0. 0174 532925- 
C 

TRAC— YPOS 
ROLL-PHIANG 
YAW- PSIANG 
DEPTH— ZPOS 
PITCH-THEANG 
B0WANG-(DB/:.  01745) 

STNANG-( DS/.  01745 ) 

100  CONTINUE 

3  WRITE(*,*)  '.NPTS  -  '  ,NUMPTS 

WRITE(*,*)  'TIMEINTERVAL  -  ',DELT 
WRITE (*,*)  'NAVIGATOR  UPDATE  TIME  -  ',NAVUPDATE 
C  WRITE(»,*)  'TARGET  RADIUS  -  '.TARGET 
STOP 
END 

***************DEPTH- CONTROL  OBSERVER******’********** 

SUBROUTINE  OBSER ( QHADOT,  THADOT-,  ZHADOT,  QHAT ,  THAT ,  ZHAT ,  DELT ,  ZPOS  ,  D 
‘S,U,)' 

QHADOT— 0.7»QHAT-0. 03*THAT-0.035*DS-20. 9293* (ZPOS-ZHAT) 
THADOT-QHATt14 . 4092* ( ZPOS-ZHAT) 

ZHADOT— 6 *THAT+16'.  4 5*  (  ZPOS-ZHAT) 

QHAT-  QHAT•^DELT*QHADOT 
•  THAT-  THAT■^DELT*THADOT 

ZHAT-  ZHAT+DELT*ZHADOT 
RETURN 
END 

.  .-.  SUBROUTINE  FOR  THE  ANGLE  ALPHA 

SUBROUTINE  ANGLE( XI , Y1 , X2 , Y2 , A) 

REAL  X1,Y1,X2, Y2,A,DX,DY 

DX-X2-X1 

DY-Y2-Y1 

A>ATAN2(DY,DX) 

RETURN 

END 

.  ...  SUBROUTINE  FOR  XLCASE  AND  YLCASE 

SUBROUTINE  HEAD (  L ,  XPOS  ,  YPOS , XP.OSl ,  YPOSl ,  ALPH ,  XLCASE ,  YLCASE ) 

REAL  XPOS , YPOS , XPOS 1 , YPOSl , ALPH , XLCASE , YLCASE , L 

YLCASE-( ( YPOS-YPOSl*L)*COS(ALPH) )-( ( XPOS-XPOSl *L ) *SIN( ALPH ) ) 

XLCASE-( (XPOS-XPOSl*L)*COS{ALPH) ) + ( ( YPOS-YPOSl * L ) *SIN ( ALPH ) ) 

RETURN 

END 

...  SUBROUTINE  FOR  DETERMINING  THE  REQUIRED  POSITION 
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SUBROUTINE  POSREQ (TIME , UREQ, XREQ ) 

REAL  XREQ, UREQ, TIME 

XREQ-UREQ*TIME 

RETURN 

END 

. . .  SUBROUTINE  FOR  NUMERICAL  INTEGRATION  USING  THE  TRAPEZOIDAL  RULE 

SUBROUTINE  TRAP( N , A , B , OUT ) 

DIMENSION  A(1):,B(1) 

Nl-N-1 
OUT-0.0 
DO  1  I-1,N1 

OUT1-0.5*(A(  I  )+A(:I  +  l )  )  *(B(I+1)-B(  I  )  ) 

OUT-OUT+OUTl 
1  CONTINUE 
RETURN 
END 
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APPENDIX  B 


C  ADDED  CURRENT  AS  A  DISTURBANCE  IN  THE  CONTROL  LAW* 

C  ADDED  CURRENT  OBSERVER 

C  ADDED  MONITORING  OF  SECOND  LEG  TO  INITIATE  TURN 

Q  iklk*********lk****Vk****************************^*****  ************ 

REAL  MASS,LATyAW,NORPIT 

REAL  MM(6,6) ,G4(4) ,GK4( 4) ,BR(9) ,HH(9) 

REAL  B(6,6),BB(6,6) 

REAL  A(  12-,  12  ) ,  AA(  12 , 12  );,  INDX(  100 ) ,  XDES  ( 100  ) ,  VDES  ( 100  ) ,  ZDES  ( 100  ) 

REAL  XPP  ,XQQ  ,XRR  ,XPR 
REAL  XUDOT  ,XWQ  ,XVP  ,XVR 
REAL  XQDS  ,XQDB  ,XRDR  ,XW 
REAL  XWW  ,XVDR  ,XWDS  ,XWDB 
REAL  XDSDS.XDBDB  ,XDRDR  .XQDSN 
REAL  XWDSN  .XDSDSN 

REAL  TIME,S,EITA|=U3AR,UHAT,C0MZ,BAR,SIM,DE,SAT,VHAT,ZZ0BS,ZZ0BSD0T,SIM1 
REAL  SSI  ,  SS2,UD,XD, YD,TD,TNWP,XA, YA,HD,HDMDEG,DAWAVr,SATSGNl ,SATSGN2 
REAL  NAVUPDATE , TNAV , TARGET , FF , GG , HHH , LLL , HDP , HDM , VCUR , UCUR , UCO , VCO , WCO 
REAL  UCOOBS,VCOOBS,VCOHAT,UCHAT 
INTEGER  DV 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  YPDOT  ,YRDOT,YPQ  ,YQR 
REAL  YVDOT  ,YP  ,YR  , YVQ 
REAL  YWP  ,YWR  ,YV  , YVW 
REAL  YDR  ,CDY 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  ZQDOT  ,ZPP,ZPR  ,2RR 
REAL  ZWDOT  , ZQ  , ZVP  , ZVR 
REAL  ZW'  ,ZW  ,ZDS  ,ZDB 
REAL  ZQN  -,ZWN  ,  ZDSN  ,CDZ 
REAL  ZHADOT.ZHAT' 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

REAL  KPDOT  , KRDOT  , KPC  /KQR 
REAL  KVDOT  ,  KP  ,KR  ,KVQ 
REAL  KWP  ,  KWR  , KV  , KVW 

REAL  KPN  ,  KDB 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

REAL  MQDOT  ,MPP  ,MPR,MRR 
REAL  MWDOT  ,  MQ  ,MVP  ,MVR 
REAL  MW  ,  MW  ,MDS  ,MDB 
REAL  MQN  ,  MWN  ,MDSN 
REAL  QHADOT,QHAT,THADOT,THAT 

YAW  HYDRODYNAMIC  COEFFICIENTS 

REAL  NPDOT,NRDOT,NPQ  ,NQR 
REAL  NVDOT  ,  NP  ,NR  ,NVQ 

REAL  NWP  ,  NWR  ,NV  ,NVW 
REAL  NDR 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 
REAL  WEIGHT  ,  BOY  ,VOL  ,XG 
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REAL  YG  ,  ZG  ,XB  ,ZB 

REAL  IX  ,  lY  ,IZ  ,IXZ 

REAL  lYZ  ,  IXY  , YB 

REAL  L  ,  RHO  ,G  ,NU 

REAL  AO  .KPROP  ,NPROP  ,  XlTEST 

REAL  DEGRUD  ,DEGSTN 

COMMON  /BLOCKl/  F(12),  FP(6),  XMMINV(6,6),  UCF 

INTEGER  N , lA , I DGT , I ER , LAST , J , K , M , J J , KK , I 

REAL  VECVl ( 9 ) , VECV2 ( 9 ) X ( 12 ) , VECHl ( 9 ) , VECH2 ( 9 ) , Xl ( 9 ) 

RUDDER  COEFFICIENTS 

PARAMETER  (  DSMAX-  -0.175) 

LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETERCXPP  -  7.E-3  ,XQQ  -  -1.5E-2  ,XRR  «  4.E-3  ,XPR  -7.5E-4, 

&  XUDOT— 7.6E-3  ,XWQ  -  -2.E-1  ,XVP  -  -3.E-3  ,XVR  -  2.E-2, 

&  XQDS-2.5E-2  ,XQDB— 2.6E-3  ,XRDR-  -l.E-3  ,XW  -5.3E-2, 

&  XWW,  -1.7E-1  .,XVDR-1.7E-3  ,XWDS-4.6E-2  ,XWDB-  l.E-2, 

5  XDSDS-  -l.E-2  ,XDBDB»  -8.E-3  ,XDRDR-  -l.E-2  ,XQDSN-  2.E-3, 

6  XWDSN-3.5E-3  ,XDSDSN-  -1.6E-3  ) 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(YPDOT-l..  2E-4-  ,  YRDOT-1. 2E-3  ,  YPQ  -  4.E-3  ,YQR  — 6.5E-3, 

4  YVDOT— 5.5E-2  ,YP  »  3.E-3  ,YR  »  3.E-2  ,YVQ  -2.4E-2, 

4  YWP  -2.3E-1  ,YWR  — 1.9E-2  ,YV  -  -l.E-1  ,YVW  -6.8E-2, 

4  YDR  -2.7E-2  ,CDY  -S.SE-l) 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER! ZQDOT— 6 .8E-3  ,ZPP  -1.3E-4  ,ZPR  -6.7E-3  , ZRR  — 7.4E-3, 

4  ZWDOT— 2.4E-1  ,ZQ  =-1.4E-l  ,ZVP  — 4.8E-2  ZVR  -4.5E-2, 

4  ZW  =•  -3.E-1  ,ZW  — 6.8E-2  ,ZDS  — 7.3E-2  ,ZDB  — 2.6E-2, 

4  ZQN  --2.9E-3  ,ZWN  — 5.1E-3  ,ZDSN-  -l.E-2  ,CDZ  -  1.0) 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER! KPDOT=  -l.E-3  , KRDOT— 3 . 4E-5  ,KPQ  — 6.9E-5  , KQR  -1.7E-2, 
4  KVDOT-1.3E-4  ,  KP  --1.1E-2  , KR  — 8.4E-4  ,KVQ— 5.1E-3, 

4  KWP  — 1.3E-4  ,  KWR  -1.4E-2  , KV  -3.1E-3  , KVW  — 1.9E-1, 

4  KPN  — 5.7E-4  ,  KDB  =  0.0  ) 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

PARAMETERIMQDOT--1.7E-2  ,MPP  -5.3E-5  ,MPR  -  5.E-3  ,MRR  — 2.9E-3, 

4  MWDOT— 6.8E-3  ,  MQ  — 6.8E-2  ,MVP  -1.2E-3  ,MVR  -1.7E-2, 

4  MW  -  l.E-1  ,  MW  — 2.6E-2  ,MDS  — 4.1E-2  ,MDB  -6.9E-3, 

4  MQN  — 1.6E-3  ,  MWN  — 2.9E-3  ,MDSN  — 5.2E-3) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER!  NPDOT— 3. 4E-5  ,  NRDOT— 3 . 4E-3  ,  NPQ  — 2.1E-2  ,NQR  -2.7E--3, 

4  NVDOT-1.2E-3  ,  NP  — 0.4E-4  ,NR  --1.6E-2  ,NVQ  =  -l.E-2, 

4  NWP  — 1.7E-2  ,  NWR  -7.4E-3  ,NV  =-7.4E-3  ,NVW  =-2.7E-2, 

4  NDR  — 1.3E-2) 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 


135 


O'OO  0  0  0,0  OOO  O  0,00 


PARAMETER!  WEIGHT  -12000.,  BOY  -12000.  ,VOL  -200.  ,XG  -  0. 


&  YG  -  0.0 

,  ZG  -  0.20 

,XB  -  0. 

,ZB  -  0.0  , 

&  IX  -  1500. 

,  lY  -  10000. 

,IZ  -  10000. 

,IXZ  -  -10.  , 

&  lYZ  -  -10. 

,  IXY  -  -10. 

,YB  -  0.0  , 

i  L  -  17.4 

,  RHO  -  1.94 

,G  -  32.2 

,NU  -  8.47E-4 

&  AO  -  2.0 

,KPROP  -  0. 

,NPROP  -  0.  , 

XITEST-  0;.l  , 

&  DEGRUD-  0.0 

,DEGS'rN-  0.0) 

INPUT  INITIAL 

CONDITIONS  HERE  IF 

REQUIRED 

OPEN(20,FILE-'MODEL1.DAT' , STATUS- 'NEW' ) 


NUMPTS-0.0 
DV-1.0 

****** *****»*********OBTAIN:  INITIAL  INFORMATION***************** 


OPEN  (30,FILE-'INITIAL.DAT' , STATUS- 'OLD' ) 

READ  (30,*)  UO,RPM 

READ  (30,*)  UD,NAVUPbATE,SIMl,DELT 

...  READ  IN  STEERING  AND  SLIDING  SURFACE  GAINS , INITIAL  CURRENTS, 
. . .  AND  SATURATION  DESIRED 

OPEN! 21, FILE- 'SMCINT.DAT' , STATUS- ' OLD' ) 

READ(21,*)  GG1,GG2,GG3,GG4,GG5 
READ!  21 ,  *')  SRI ,  SP2  ,  SP3  ,  SP4  ,  SE5 
READ! 21,*)  UeO,VCO,WCO 
READ!21,*)  AKN,SSPHI 
READ! 21,*)  IPTS 

. . .  READ  IN  WAY  POINTS 

IF  (IPTS  .GT.  100)  IPTS-100 
DO  200  1=1, IPTS 

READ!30,*)  XD,YD,ZD 
XDES!I)-XD*L 
YDES!I)-YD*L 
ZDES!I)-2D*L 
200  CONTINUE 

PI-4.0*ATAN!1.0) 

UC-0.0 
VC-0.0 
UCOOBS-0.0 
VCOOBS-O.O 
VO  -  0.0 
WO  -  0.0 
PO  -  0.0 
QO  i  0.0 
RO  -  0.0 
PHIO  -  0.0 
THETAO  -  0.0 
PSIO  -  0.0 
XPOSO-0.0 
YPOSO-0.0 
ZPOSO-0.0 
XDl-0.0 
YDl-0.0 
Zl-O.Q 
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Z19-0.0 

Z2-0.0 

z2a-o.o= 

Z3-0.0 
Z39-0.0 
D3-  0.0 
DS  -  0.0 
DR  -  0.0 
LATVAW  ^  0.0 
NORPIT  -  0.0 
RE  -  U0*L/NU 
TNAV-0 
XA-XPOSO 
VA-ypdso 

TIME-0.0 
TIMEO-O.O 
U  -  UO 
V  -  VO 
W  -  WO 
P  -  PO 
Q  -  QO 
R  -  RO 

XPOS  -  XPOSO 

Ypos  -  yposo 

ZPOS  -  ZPOSO 
P'SI  -  PHIO 
THETA  -■  THETAO 
PHI  -  PHIO 
QHADOT-0.0 
THADOT-O.O 
ZHADOT-0.0 
QHAT-0.0 
THAT-0.0 
ZHAT-0.0 
VHAT-0.0 
ZOBSDOT-0.0 
ZZOBS  -  0.0 

DEFINE  THE  LENGTH  X  AND  HEIGHT  HH  TERMS  FOR  THE  DRAG  INTEGRATION 

Xl(l)  -  -105.9/12. 

Xl(2)  -  -99.3/12. 

Xl(3)  -  -87.3/12. 

Xl(4)  -  -66.3/12. 

Xl(5)  -  72.7/12. 

Xl(6)  -  83.2/12. 

Xl(7)  -  91.2/12. 

Xr(3)  -  99.2/12. 

Xl(9)  -  103.2/12. 

HH(1)  -  0.00/12. 

HH(2)  -  8.24/12. 

HH(3)  -  19.76/12. 

HH(4)  -  29.36/12. 

HH(5)  -  31.85/12. 

HH(6)  -  27.84/12. 

HH(7)  -  21.44/12. 

HH(8)  -  12.00/12. 

HH(9)  -  0.00/12. 
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BRd)  - 

0.00/12.0 

BR(2)  - 

8.24/12.0 

BB(3)  - 

1 9.-7  6/1 2.0 

BB(;4)-  - 

29.36/12.0 

BB(5)  - 

31.85/12.0 

BR(6)  - 

27.84/12.0 

BB(7)  - 

21.44/12.0 

BR(8)  - 

12.00/12.0 

BB(9) 

0.00/12.0 

MASS  -  WEIGHT/G 

N  “  6 

DO  15  J 

-  1,N 

DO  10  K  -  1-,N 
XMMINV(J,K)  -  0.0 
MM(J,K)  -  0.0 
id-  CONTINUE 

15  CONTINUE 
C 

MM(1,1)  -  MASS  -< (RHO/2)*(L**3)*XUDOT) 

MM(i,5)  -  KASS*ZG 
MM(1'^6)  -  -MASS*YG 
G 

MM(2,2)  -  MASS  -(.(RHO/2)*tt»*3)*yVDOT) 

MM(2,4)  -  -MASS*ZG  -( ( BHO/2 ) * (L* *4 ) *yPDOT ) 
MM(2,6).  -  MASS*XG  -  ( ( RHO/2 ) * ( L**4 ) ‘YRDOT) 

C 

MM(3,3)  -  MASS  -  (.( RHO/2 )  *  ( I,**3  )>ZWDOT) 

MM(3,4)  -  MASS*yG 

MM(3,5)  -  -MASS*XG  -( ( HHO/2 ) * ( L* *4 ) *ZQDOT) 

C 

MM(4,2)  -  -MASS*ZG  -  ((RHO/2)*(L*»4)*KVDOt) 
MH(4, 3)  -  MASS*yG 

MM (4, 4)  -  IX  -  ( (RHO/2)*(L**5)*KPDOT) 

mm(4,5)  -  -ixy 

MM(4,6)  -  -IXZ  -( (RHO/2)*(L»*5)*KRDOT) 

C 

MM(5,1)  -  MASS*ZG 

MM(5,3)  -  -MASS*XG  -( ( RHO/2 ) * ( L* *4 ) *MWDOT) 
MM(5,4)  -  -ixy 

MM(5,5)  -  ly  (RHO/2)*(L*»5)*MQDOT) 

MM(5^6)  -  -lyz 
C 

MM(6,1)  -  -MASS*YG 

MM(6,2)  -  MASS*XG  -((RHO/2)»(I,**4)*NVDOT) 
MM{6,4)  -  -IXZ  -  ((RH0/2)*(L**5)»NPD0T) 

MM(6,5)  -  -lYZ 

MM(6,6)  -  IZ  -  ( (RH0/2)*(L**5)»NRD0T) 

C 

C  OBSERVER  POLES 
C 

SI  — 1.0 

52—  1.1 

53—  1.2 
C 

C  OBSERVER  A  MATRIX  CONSTANTS  AND  3  MATRIX  CONSTANTS 
C 


138 


All— 0.04538 
A12— 0.35119 
A21— 0.002795 
A22— 0.09568 
B1  »  0.011432 
B2  —0.004273 
C 

C  *****ROUTINE  FOR  INVERTING  THE  MM  MATRIX***** 

C 

DO  12  I-1,N 
DO  11  J-1,N 

XMMINV(I,0)-0.0 

11  CONTINUE 
XMMINV(I,i)-l 

12  CONTINUE 

CAul  INVTA(MM,N,INDX,D) 

DO  13  J-1,N 

CALL  INVTB(MM,N,INDX,XMMINV(1,J) ) 

13  CONTINUE 
C 

C  ************  INPUTS  ************* 

C 

C  RUDDER  AND  DIVE  PLANE  COMMANDS 

C 

SIM-SIMl/DELT 
TIME-0.0 
DS-  0.0 
DR-  0.0 
DB-  0.0 
EI.TA-4.0 
BAR- .4 
C 

YINTGR-O.O 
SSPHM— SSPHI 
C  • 

iSIM-SIMl/DELT 
I START- 1 
C 

C  ***************«***SIMULATION  BEGINS  **************** 

C 

C  LOOP  OVER  WAY  POINTS 
C 

DO  210  IP-1, IPTS 

IF  (IP  .GE.  2)  GO  TO  211 
XD-XDES(l) 

YD-YDES(l) 

XDl-0.0 
YDl-0.0 
XD2-XD 
YD2^YD 
GO  TO  212 

211  XD-XDES(IP) 

YD-YDES(IP) 

XD1-XD2 

YD1-YD2 

XD2-XD 

YD2-YD 

212  YD12-(YD2-YD1) 

XD12-{XD2-XD1) 

ALPH-ATAN(YD12/XD12) 


139 


non  no  o 


ALPH-ABS(ALPH) 

IF  ((XD12  .GE.  0.0)  .AND.  (YD12  .GE.  0.0))  ALPH-  ALPH 

IF  ((XD12  .GE.  0.0)  .AND.  (yD12  .LT.  0.0))  ALPH-  -ALPH 

IF  (  (XDr2  .LT.  0.0)  .AND.  (yD12  .GE.  0 . 0 )  )  ALPH-PI-ALPH 

IF  ((XD12  .LT.  0.0)  .AND.  (;yD12  .LT.  0 . 0 )  )  ALPH-PI+ALPH 

VCHAT-VC*C0S(ALPH)-UC*SIN(ALPH) 
UCHAT-VC*SIN(ALPH)+UC*COS(ALPH) 

ycTE- ( ypos-yoi ) *cos ( alph ) - { xpos-xdi ) *  s i n ( alph ) 

XCTE- ( ypos-yoi ) *SIN( alph ) +( xpos-xdi ) *COS (alph ) 

Z2-VCHAT+S2*YCTE 
Z  3-UCHAT+S  3  *XCTE+0*COS ( PSI-ALPH ) 

4  WRITE(*,101)  XD/L,yD/L 

WRITE( * ,102  )  XD12/L,  yD12/L, ALPH* (180/PI ) , ISTART 

101  FORMATC  HEADING  FOR  (X,y)  -  ( ' , F9 . 3 . ' , ' , F9 . 3 , ' ) ' ) 

102  FORMAT('  XD12-  ' , F8 . 3 , '  yD12-  ',F8.3,'  ALPH-  ' , F9 . 3 , '  ISTART- 

S  I6) 

DO  100  M-ISTART, ISIM 
ICOUNT-M 

PROPULSION  MODEL 
SIGNU  »  1.0 

IF  (U.LT.O.O)  SIGNU  -  -1.0 
IF  (ABS(U) .LT.XITEST)  U  -  XITEST 
SIGNN  -  r.o 

IF  (RPM.LT.O.O)  SIGNN  »  -1.0 
ETA  -  0.012*RPM/U 
RE  -  U*L/NU 

CDO  -  .00385  +  (1.296E-l7.)i(RE  -  1.2E7)**2 

CT  -  ABS(0.008*L**2*ETA*ABS(ETA)/{A0) ) 

CTl  -ABS(  0.008*L**2/(A0) ) 

EPS  -  -1.0+SIGNN/SIGNU*(SQRt(CT+1.0)-1.0)/(SQRT(CTl+1.0)-1.0) 
XPROP  -  CD0*(ETA*ABS(ETA)  -  1.0) 

,. .  CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
DO  500  K-1,9 

UCF-(V+X(K)*R)*»2+(W-X(K)*Q)**2 

UCF-SQRT(OCF) 

IF  (UCF.LT.l.E-6)  GO  TO  601 

CFLOW  -CDy*HH(K)*(V+X(K)*R)**2+CDZ*BR(K)*(W-X(K)»Q)**2 
VECHl ( K ) -CFLOW* ( V+X( K ) *R )/UCF 
VECH2(K) -CFLOW* (V+X(K)*R)*X(K)/UCF 
VECVl ( k ) -CFLOW* ( W-X ( K ) *Q ) /UCF 
VECV2 ( K ) -CFLOW* ( W-X ( K ) *Q ) *X( K )/UCF 
500  CONTINUE 

CALL  TRAP (9, VECVl, X, HEAVE) 

CALL  TRAP(9,VECV2,X,PITCH) 

CALL  TRAP( 9, VECHl, X, SWAY  ) 

CALL  TRAP(9,VECH2,X,yAW  ) 

SWAY— 0 . 5*RH0*SWAY 
YAW  — 0.5*RHO*YAW 
HEAVE— 0  .■S*RHO*HEAVE 
PITCH-+0 . 5*RHO*PITCH 
GO  TO  602 
601  HEAVE-O.O 
PITCH-0.0 
SWAY  -0.0 
YAW  -0 . 0 
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602  CONTINUE 

EORCE  EQUATIONS 


LONGITUDINAL  FORCE 

FP(1)  -  MASS*V*R  -  MASS*W*Q  +  MASS*XG*Q**2  +  MASS*XG*R**2- 
&  MASS*YG*P*Q  -  MASS*ZG*P*R  +  ( RHO/2 ) *L**4* (XPP*P*»2  + 

4  XQQ*Q**2  +  XRR*R**2  +  XPR*P*R)  +( RHO/2 ) *L**3* ( XWQ*W*Q  + 

4  -XVP*V*P+XVR*V*R+U*Q*(XQDS*DS+XQDB*DB)+XRDR*U*R*DR)+ 

4  (RHO/2)*L**2*(XVV*V»*2  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W» 

4  (XWDS*DS+XWDB*DB)+U**2*(XDSDS*DS**2+XDBDB*DB**2+ 

4  XDRDR*DR**2) )-(WEIGHT  -BOY) *SIN(THETA)  +( RHO/2 ) *L** 3* 

4  XQDSN*U*Q*DS*EPS+(RH0/2)*L**2*(XWDSN*U*W*DS+XDSDSN*U**2* 

4  DS**2)*EPS  +( RHO/2 )*L**2*U**2*XPR0P" 

LATERAL  FORCE 

FP(2)  -  -MASS*U*R  -  MASS*XG*P*Q  +  MASS*YG*R**2  -  MASS*2G*Q*R  + 

4  (RHO/2)*L**4»(YPQ*P*Q  +  YQR*Q*R ) +  { RHO/2 ) *L** 3*  ( YP»U*P  + 

4  YR*U*R  +  YV0*V*Q  +  YWP*W*P  +  YWR*W*R)  +  ( RHO/2 ) *L**2* 

4  (YV*U*V  +  YVW*V*W  +YDR*U*»2*DR)  +SWAY  +(WEIGHT-BOY) » 

4  COS( THETA) *SIN( PHI )+MASS*W*P+MASS*YG*P*»2 

NORMAL  FORCE 

FP(3)  -  MASS*U*Q  -  MASS*V*P  r  MASS*XG*P*R  -  MASS*YG*Q*R  + 

4  MASS*ZG*P*«2  +  MASS*ZG*Q**2  +  ( RHO/2 ) •L*«4 * ( ZPP*P»* 2  + 

4  2PR*P»R  +  ZRR»R**2)  +  ( RHO/2 ) *L**3* ( ZQ»U*Q  +  ZVP?V*P  + 

4  ZVR»V*R)  +.( RHO/2  )*L**2*{ZW*U*W  +  ZW*V»»2  +  U**2*(ZDS* 

4  DS+ZDB»DB ) ) +HEAVE+ {WEIGHT-BOY ) *COS (THETA ) *COS ( PHI ) + 

4  (RHO/2) *L**3*ZQN*U*Q»EPS  +( RHO/2 ) 'L* *2* ( ZWN«U*W  +ZDSN* 

4  U**2*DS)»EPS 

ROLL  FORCE 

FP(4)  »  -IZ*Q»R  +IY*Q*R  -IXY*P*R  +IYZ*Q*«2  -IY2*R*»2  +IX2»P»Q  + 
4  MASS»YG»U*Q  -MASS»YG*V»P  -MASS*ZG*W*P+( RHO/2 ) *L* *5*  (  KPQ* 

4  P«Q  +  KQR»Q*R)  +{  RHO/2 ) ‘L* *4 * ( KP*U»P  +KR*U*R  +  KVQ*V*Q  + 

4  KWP»W»P  +  KWR»W*R)  +(RH0/2)*L»»3*{KV*U*V  +  KVW»V*W)  + 

4  (YG*WEIGHT  -  YB»BOY) *COS( THETA) ‘COS ( PHI )  -r  (ZG»WEIGHT  - 

4  ZB*BOY)*COS(TKETA)*SIN(PHI)  +  ( RHO/2 ) *L**4»KPN»U*P*EPS-r 

4  (RHO/2)»L*»3*U**2»KPROP  +MASS*ZG*U*R 

PITCH  FORCE 

FP(5)  -  -IX*P*R  +IZ*P»R  +IXY*Q*R  -IYZ*P*Q  -IXZ*?**2  +IXZ*R**2  - 
4  MASS*XG*U»Q  +  HASS*XG*V*P  +  MASS*ZG»V*R  -  MASS»ZG*W»Q  + 

4  {RH0/2)*L»*5»(MPP»P*»2  +MPR*P*R  +MRR»R**2 )  +  {  RHO/2 ) ‘L* *4 • 

4  (MQ*U*Q  +  MVP*V*P  +  MVR*V*R)  +  ( RHO/2 ) *L**3* ( MW*U*W  + 

4  MW*V*»2+U**2*{MDS*DS+MDB*DB) )+  PITCH  -(XG*KEIGHT- 

4  XB»BOY)*COS(THETA)*COS{PHI)+(RHO/2)*L*»4*MQN*U*0*EPS  * 

4  ( RHO/2 )*L**3*( MWN*U*W+MDSN*U**2*DS)*EPS- 

4  (ZG*WEIGHT-ZB*B0Y)*SIN(THETA) 

YAW  FORCE 

FP(6)  -  -IY»P*Q  +IX»P*Q  +IXY*P**2  -IXY*Q*»2  +IY2»P*R  -IXZ»Q»R  - 
4  KASS*XG*U*R  +  MASS*XG*W»P  -  KASS»YG*V»R  +  MASS»YG»W»Q  + 
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&  (RH0/2)*L**5*(NPQ*P*Q  +  NQR*Q*R)  +( RHO/2 ) *L**4*  ( NP*U*P+ 

&  NR*U*R  +  NVQ*V*Q  +NWP*W*P  +  NWR*W*R)  +{ RHO/2 ) *L**3*  ( NV* 

&  U*V  +  NVW*V*W  +  NDR*U**2*DR)  +  YAW  +  (XG*WEIGHT  - 

&  XB*BOY) *COS( THETA) *SIN( PHI )+(YG*WEIGHT)-*SIN(THETA) 

&  +(RH0/2)*L**3*U**2*NPR0P-YB*B0Y*SIN(THETA) 


NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  -  1,6 

F(J)  -  0.0 
DO  600  K  -  1,6 

F(J)  -  XMMINVI J,K)*FP(K)  +  F(J) 

CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 
INERTIAL  POSITION  RATES  F(7-9) 

F(7)  -  UCO  +  U*COS(PSI )*COS(THETA)  +  V* ( COS( PSI ) *SIN( THETA) * 

4  SIN(PHI  )  -  SIN(PSI)*COS(PKI)  )  +  W* ( COS (PSI ) ‘SINC THETA) * 

4  COSCPHI)  +  SIN(PSr)*SIN(PHl) ) 

F(0)  -  VCO  +  U»SIN( PSI ) *COS(THETA)  +  V* { SIN( PSI)  *SIN(THETA) • 

4  SIN(PHI)  +  COS(PSr)*COS{PHI)  )  +  W» ( SIN{ PSI ) *SIN( THETA) • 

4  COS(PHI)  -  COS(PSI)*SIN(PHI).) 

F(9)  =>  WCO  -  U*SIN{THETA)  +V*COS( THETA) *SIN( PHI )  +W* COS ( THETA ) • 
4  COS (PHI) 

EULER  ANGLE  RATES  F(10-12) 

F(10)  -  P  +  Q»SIN(PHI)*TAN(TKETA)  +  R*COS ( PHI ) *TAN( THETA) 

F(ll)  -  Q*COS(PHI)  -  R*SIN(PHI) 

F(12)  «  Q*SIN(PHI)/COS(THETA)  +  R*COS ( PHI )/COS( THETA) 


UDOT  -  F(I> 

VDOT  -  F(2) 

WDOT  -  F(3) 

PDOT  -  F(4) 

QDOT  -  F(5) 

ROOT  -  F(6) 

XDOT  -  F(7) 

YDOT  -  r(8) 

ZDOT  -  F(9) 

PHIDOT  -  F(10) 

THETAD  -  F(ll) 

PSIDOT  -  F(12) 

•***••*  CREATE  OUTPUT  DATA  FILE 

IF  (M  .EQ.  DV  )  THEN 
TIMER-FLOAT(M)/2. 

WRITE  (20,*)  M 

IF  (DR  .GT.  0.4)  THEN 
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DR  «  0 . 4 

ELSEIF  (DR  .LT.  -0.4)  THEN 
DR  -  -0.4 
ENDIF 
C 

WRITE  (20,743)  DR/. 01745 

WRITE  (20,744)  XPOS/L, YPOS/L,XDES( I?)/L, YDES( IP)/L 
WRITE  (20,746)  ( PSI-ALPH )/. 01745 , YCTE/L 
C 

IF  (DR99  .GT.  0.4)  THEN 
DR99  -  0.4 

ELSEIF  (DR99  .LT.  -0.4)  THEN 
DR99  -  -0.4 
ENDIF 
C 

WRITE  (20,744)  YCTE99/L,DR99/0 . 01745, SS99 , PROD 

743  FORMAT  (Ell. 3) 

744  FORMAT  (4E12.4) 

746  FORMAT  (2E12.4) 

C 

NUMPTS-NUMPTS  +  1 

DV-DV-rl.O/DELT 

ENDIF 

*  «  *  *  * 


FIRST  ORDER  INTEGRATIO" 


u  i  U  +  DELT*UDOT 

U 

-  SURGE  RATS 

V  *  V  +  bELT*VDOT 

V 

-  SWAY  RATE 

W  ■  W  +  DELT*WD0T 

W 

-  HEAVE  RATE 

?  »  ?  +  DELT*PDOT 

? 

-  ROLL  RATE 

Q  -  Q  +  DELT»QDOT 

Q 

-  PITCH  RATE 

R  -  R  +  DELT'RDOT 

R 

-  YAW  RATE 

XPOS  -  XPOS  +  DELT»XDOT 

X 

-  SURGE 

YPOS  -  YPOS  +  DELT*YDOT 

Y 

-  SWAY 

2P0S  -  ZPOS  +  DELT»2D0T 

2 

-  HEAVE 

PHI  -  PHI  +  DELT*PHIDOT 

PHI  -  ROLL 

THETA  -  THETA  +  DELT*TKETAD 

THETA  -  PITCH 

PSI  -  PSI  +  DELT*PSIDOT 

PS 

I  -  YAW 

YINTGR-YINTGR  +  DELT*YLCASE 

INTEGRAL  OF  LATERAL 

DEVIATION  ERROR 

••♦••♦•••SLIDING  MODE  DEPTH  CONTROL*** . •*•• 

CALL  OBSER(QKnDOT,TKADOT,ZKADOT,QHAT.TKAT.EHAT,DSLT,SPOS.DS,U0l 
C 

S-QHAT  *  0.52»THAT  -  0.O112»C2HAT-CO.MZ*LI 
irC.ABSiS)  .LT.  BAR)  SAT-(S/BAR] 
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IF(S  .LE.  -BAR)  SAT— 1.0 

IF(S  .GE.  BAR)  SAT-1.0 

UHAT— 5.1429*QHAT  +  1.0714*THAT 

UBAR-EITA*SAT 

DE-UHAT+UBAR 

IF  (DE  .GE.  0.4)  DS-0.4 

IF  (DE  .LE.  -0.4)  DS  — 0.4 

IF(  (DE  .LT.  0.4)  .AND.  ( DE  .GT.  -0.4))  DS-DE 
DB— DS*1.0 

*********SLIDING  mode  STEERING  CONTROL************ 


*******pi:,anner***  ****** 


.  ..  DETERMINE  REQUIRED  POSITION 

YCTE-  ( YPOS-YDI )  *COS  ( .%LPH )  -  ( XPOS-XDl )  *  SIN  ( ALPH ) 

XCTE- ( YPOS-YDt)  *SIN ( ALPH )  +  ( XPOS-XDl ) *COS ( ALPH ) 

.  ..  DETERMINE  IF  XLCASE  IS  WITHIN  L/2  DISTANCE  OF  D 

XT-SQRT( (XD2-Xbl)**2  +  ( YD2-YD1 ) *  *  2 ) 

XAWAY- ( XT-XCTE ) 

DAWAY-ABS ( XAWAY ) 

************************* *CURRENT  OBSERVER******************************* 


Z1D0T-S1*21+(S1*(A11+A22)/A21+(A12-A11*A22/A21)*U- 
$  S1*S1/(A21*U))*R+(B1*U-B2*(A11*U-S1)/A21)*U*DR  * 

Z2D0T«S2*Z1+S2*Z2+S2*0*SIN(PSI-ALPH)-S2*S2*YCTE+ 

$  (S2*(A11*U-S1)-/(A21*U)  )*R 

Z3D0T-S3*Z3-S3*S3*XCTE 

Zl-ZlfZlDOT*bELT  t 

Z2-Z2+Z2D0T*DELT 

Z3-Z3+Z3DOT*DELT 

VHAT-Z1+R*(A11*U-S1)/(A21*U) 

VCHAT-Z2-S2*YCTE 

UCHAT-Z3-S3*XCTE 

VCOOBS-VCHAT 

UCOOBS-UCHAT-U*COS ( PSI-ALPH ) 

VC-UCOOBS*SIN( ALPH )+VCOOBS*COS( ALPH) 

UC-UCOOBS  *COS ( ALPH ) -VCOOBS  *  S IN ( ALPH ) 

IF  (IP  .LT.  IPTS)  GO  TO  250 
IF  (DAWAY  .LT.  0.1)  GO  TO  201 
GO  TO  230 

MONITOR  CONTROL  LAW  FOR  NEXT  SEGMENT 


250  YN12-YDES( IP+1 )-YDES( IP) 
XN12-XDES( IP+1)-XDES( IP) 

B  ETA- ATAN ( YN 1 2/XN 1 2 ) 
BETA-ABS(BETA) 

IF  ((XN12  .GE.  0.0)  .AND. 

IF  ( (XN12  .GE.  0.0)  .AND. 

IF  ((XN12  .LT.  0.0)  .AND. 

IF  ((XN12  .LT.  0.0)  .AND. 


(YN12  .GE.  0.0))  BETA-  BETA 
(YN12  .LT.  O.OJ)  BETA-  -BETA 
(YN12  .GE.  0.0))  BETA-PI-BETA 
(YN12  .LT.  0.0))  BETA-PI+BETA 


. . .  CURRENT  OBSERVER  FOR  NEXT  PATH 
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YCTE99- ( yPOS-YDES (IP)) *COS ( BETA ) - { XPOS-XDES ( I P ) ) *SIN ( BETA ) 
VC99-VC*COS(BETA)-UC*SIN(BETA) 

VC2-VC99/U 

IF  (VC 2  .GE.  1.0,)  THEN 
VC2-1 .0 

ELSEIF  (VC2  .LE.  -1.0)  THEN 
VC2  — 1.0 
ENDIF 
C 

SS99-SP1*(PSI-BETA)+SP2*VHAT+SP3*R+SP4*YCTE99+ 

?  ( (SSPHI*GG1)/AKN+SP1)*ASIN(VC2) 

IF  (ABS(SS99)  .LT.  SSPHI)  SPHI99-SS99/SSPHI 
IF  (SS99  .LE.  SSPHM)  SPHI99— 1.0 
IF  (SS99  .GE.  SSPHI)  SPHI99-  1.0 
DR98-GG1*(PSI-BETA)+GG2*VHAT+GG3*R+GG4*YCTE99 
DR97-AKN*SPHI99 
DR99-DR97+DR98 
IF  (DR99  .GE.  0^4)  DR99-0.4 
IF  (DR99  .:LE.  -0.4)  DR99— 0.4 
DRNEW-DR99 
C 

IF  (M  .EQi  1)  GO  TO  230 
PROD-DROLb*DRNEW 
C 

IF  (XAWAY  .LE.  0.0)  GO  TO  201 
IF  (XAWAY  .GT.  (.0.5*XT))  GO  TO  230 
IF  (PROD  .LE.  0.0)  GO  TO  201 
C 

230  DROLD-DRNEW 

******NAVIGATOR’' ****** 

IF  ((TIME-TNAV)  .GE.  NAVUPDATE  )  THEN 
XA«XID 
YA-YID 

TNAV-TNAV+NAVUPDATE 
ENDIF 

*** ‘HEADING* ******** 

****DESIRED  SPEED*** 

UD  IS  SPECIFIED  AND  HELD  CONSTANT 

********RPM  input  calculation  ********* 

SSl-U-UD 

IF(ABS(SS1)  .LT.  1.0)  SATSGN1-(SS1/SSPHI) 

IF(SS1  .LE.  SSPHM)  SATSGNl'i-1 . 0 
IF(SS1  .GE.  SSPHI)  SATSGNl-  1.0 
RPM— 1153.9*SATSGN1  +  83.33*U 
IF  (RPM  .GE.  500.0)  RPM-  500.0 
IF  (RPM  .LE.  -500.0)  RPM— 500.0 

*******rUDDER  input  CALCULATION******* 

************BEGIN  SMC  CALCULATIONS  ********************* 

DANGLE-(PSI-ALPH) 

VCl-VCOOBS/U 
IF  (VCl  .GE.  1.0)  THEN 
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vci-i.o; 

ELSEIF  (VCl  .LE.  -1.0)  THEN 
VCl  — 1.0 
END  IF 
C 

SS2-SP1 * ( DANGLE ) +SP2  *VHAT+SP3  *R+SP4  * YCTE+ 

&  ( (SSEHI*GG1)/AKN  +  SPl ) *ASIN( VCl ) 

C 

IF(ABS(SS2)  .LT.  SSPHI )  SATSGN2- ( SS2/SSPHI ) 

IF(SS2  .LE.  SSPHM)  SATSGN2— T.O 
.  IF(SS2  .GE.  SSPHI)  SATSGN2-1.0 

C 

bR-AKN*SATSGN2+GGl *( DANGLE )+GG2*VHAT+GG3*R+GG4*YCTE'fGG5*yiNTGR 
C 

IF  (DR  .GE.  0.4)  DR  -  0.4 

IF  (DR  .LE.  -  0.4)  DR  -  -0.4 
C 

TIME-TIME+DELT 

C 

PHIANG  -  PHI/O. 0174532925 
THEANG  -  THETA/0.0174532925 
PSIANG  -  PSI/0. 0174532925 
C 

ALPANG-ALPH/0 .0174532925 
C 

TRAC— YPOS 

ROLL-PHIANG 

YAW-PSIANG 

DEPTH— ZPOS  * 

PITCH-THEANG 

BOWANG-(DB/. 01745) 

STNANG-(DS/.01745) 

100  CONTINUE  -t- 

GO  TO  300 

201  ISTART-ICOUNT 

WRITE(*,103j  YCTE99 

103  FORMAT('  YCTE99-  ',F9.3) 

WRITE(*,104)  SS99 

104  FORMAT('  SS99-  ',F9.3) 

WRITE( *,105)  DR99 

105  FORMATC  DR99-  ',F9.3) 

WRITE (  *,106):  DRNEW 

106  FORMAT('  DRNEW-  ',F9.3) 

WRITE(  *,107).  DROLD 

107  FORMAT('  DROLD-  ',F9.3) 

WRITE(*,108)  PROD 

108  FORMATC  PROD-  '  ,  F9 . 3  ) 

210  CONTINUE 

C  400  CONTINUE 

300  WRITE(*,*)  'NPTS  -  ',NUMPTS 

WRITE(*,*)  'TIMEINTERVAL  -  ',DELT 
WRITeI*,*)  'NAVIGATOR  UPDATE  TIME  -  ' ,NAVUPDATE 
C  WRITE(*,*)  'TARGET  RADIUS  -  ', TARGET 

STOP 

END  ^ 

***»*******»***deptH  control  OBSERVER***************** 

SUBROUTINE  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , DELT , ZPOS , D 
*S,U) 
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QHADOT=-0.7*QHAT-0.03*THAT-0.035*DS-20.9293*{ZPOS-ZHAT) 

THADOT-QHAT-14.4092*(ZPOS-ZHAT) 

Z  HADOT— 6  *  THAT+ 1 6 . 4  5  *  ( Z  POS-  ZHAT ) 

C 

QHAT-  QHAT+DELT*QHAD6t 
THAT-  THAT+DELT*THADOT 
ZHAT-  ZHAT+DELT*ZHADOT 
RETURN 
-END 

. . .  SUBROUTINE  FOR  THE  ANGLE  ALPHA 

SUBROUTINE  ANGLE( XI , Yl , X2 , Y2 , A) 

REAL  XI, Y1,X2,Y2,A,DX,DY 

DX-X2-XI 

DY-Y2-Y1 

A-ATAN2(DY,DX) 

RETURN 

END 

. . .  SUBROUTINE  FOR  XLCASE  AND  YLCASE 

SUBROUTINE  HEAD( L, XPOS , YPOS , XPOSl , YPOSl , ALPH , XLCASE, YLCASE ) 

REAL  XPOS , YPOS , XPOSl , YPOSl , ALPH , XLCASE , YLCASE , L 

YLCASE=( ( YP0S-YP0S1*L)*C0S(ALPH) )-( ( XPOS-XPOSl *L ) «SIN{ ALPH ) ) 

XLCASE- ( (XPOS-XPOSl*t) ‘COS (ALPH) )  +  ((yPOS-YPOSl *L).*SIN  (ALPH) ) 

RETURN 

END 

...  SUBROUTINE  FOR  NUMERICAL  INTEGRATION  USING  THE  TRAPEZOIDAL  RULE 

SUBROUTINE  TRAP ( N, A, B , OUT) 

DIMENSION  A(1),B(1) 

Nl-N-1 
•  OUT-0.0 

DO  1  I-1,N1 

OUT1-0.5*(A( I )+A( I  +  l) )*(B(I+1)-B(I)  ) 

OUT-OUT+OUTl 
1  CONTINUE 
RETURN 
END 
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